Skip to content

Commit

Permalink
fix some small thing
Browse files Browse the repository at this point in the history
  • Loading branch information
lixin committed Jan 23, 2019
1 parent a1c2b10 commit 05e4391
Show file tree
Hide file tree
Showing 3 changed files with 44 additions and 69 deletions.
46 changes: 12 additions & 34 deletions demo/KITTI.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ int main(int argc, char **argv) {
LoadImages(PathToSequence, vstrImageLeft, vstrImageRight, vTimestamps);


const int nImages = vstrImageLeft.size();
const int nImages = static_cast<const int>(vstrImageLeft.size());

//// Create VO system.
StereoVO::Config::setParameterFile(ParameterFile);
Expand All @@ -63,7 +63,7 @@ int main(int argc, char **argv) {

// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
vTimesTrack.resize((unsigned long) nImages);

cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
Expand All @@ -72,15 +72,13 @@ int main(int argc, char **argv) {
char text[100],text_t[100],text_times[100];
Mat traj = Mat::zeros(1000, 600, CV_8UC3);

// Main loop
Mat imLeft, imRight;

//error
double e_ATE = 0,e_RMSE = 0;
int ei = 0;

int begin = 0;

// Main loop
for(int ni=begin ; ni<nImages; ni++)
{
TicToc time;
Expand Down Expand Up @@ -112,30 +110,11 @@ int main(int argc, char **argv) {



// std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

// Pass the images to the SLAM system
// SLAM.TrackStereo(imLeft,imRight,tframe);
track->addFrame(pFrame);
cv::imshow("Image", imLeft);
// cv::waitKey( 30 );
if(cv::waitKey( 1 ) == 27) break;

// std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

// double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

// vTimesTrack[ni]=ttrack;

// Wait to load the next frame
// double T=0;
// if(ni<nImages-1)
// T = vTimestamps[ni+1]-tframe;
// else if(ni>0)
// T = tframe-vTimestamps[ni-1];
//
// if(ttrack<T)
// usleep((T-ttrack)*1e6);
Mat R_c_w = track->curr_->T_c_w_.colRange(0,3).rowRange(0,3),
t_c_w = track->curr_->T_c_w_.colRange(3,4).rowRange(0,3);
Mat ret = -R_c_w.inv()*t_c_w;
Expand All @@ -156,14 +135,14 @@ int main(int argc, char **argv) {
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]) +
(cam_t.z - truths[ni][2]) * (cam_t.z - truths[ni][2]));
Mat t = ( cv::Mat_<double> ( 3,1 ) <<-truths[ni][0],-truths[ni][1],-truths[ni][2]);
Mat R = ( cv::Mat_<double> ( 3,3 ) <<truths_R[ni][0],truths_R[ni][1],truths_R[ni][2],
Mat t_e = ( cv::Mat_<double> ( 3,1 ) <<-truths[ni][0],-truths[ni][1],-truths[ni][2]);
Mat R_e = ( cv::Mat_<double> ( 3,3 ) <<truths_R[ni][0],truths_R[ni][1],truths_R[ni][2],
truths_R[ni][3],truths_R[ni][4],truths_R[ni][5],
truths_R[ni][6],truths_R[ni][7],truths_R[ni][8]);
t = -R.inv()*t + R.inv()*t_c_w;
R = R.inv()*R_c_w;
t_e = -R_e.inv()*t_e + R_e.inv()*t_c_w;
R_e = R_e.inv()*R_c_w;
Mat rvec;
cv::Rodrigues(R,rvec);
cv::Rodrigues(R_e,rvec);
double e_r = cv::norm(rvec);
e_r += cv::norm(t);
e_RMSE += e_r;
Expand Down Expand Up @@ -229,9 +208,9 @@ void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
string strPrefixLeft = strPathToSequence + "/image_0/";
string strPrefixRight = strPathToSequence + "/image_1/";

const int nTimes = vTimestamps.size();
vstrImageLeft.resize(nTimes);
vstrImageRight.resize(nTimes);
const int nTimes = static_cast<const int>(vTimestamps.size());
vstrImageLeft.resize((unsigned long) nTimes);
vstrImageRight.resize((unsigned long) nTimes);

for(int i=0; i<nTimes; i++)
{
Expand Down Expand Up @@ -298,9 +277,8 @@ void LoadTracks(const string &GroundtruthFile, vector<vector<double>> &truths, v
if (!fTruth)
{
std::cout << "can not open GroudtruthFile" << endl;
// return -1;
return ;
}
double t;
while (!fTruth.eof())
{
double t[12];
Expand Down
12 changes: 6 additions & 6 deletions src/frame.cc
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
#include <memory>

//
// Created by lixin on 18-12-19.
//
Expand All @@ -7,26 +9,24 @@
namespace StereoVO
{
Frame::Frame()
: id_(-1), time_stamp_(-1), camera_(nullptr), is_key_frame_(false)
: id_(static_cast<unsigned long>(-1)), time_stamp_(-1), camera_(nullptr), is_key_frame_(false)
{

}

Frame::Frame ( long id, double time_stamp, Mat T_c_w, Camera::Ptr camera, Mat left, Mat right )
: id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), left_(left), right_(right), is_key_frame_(false)
: id_(static_cast<unsigned long>(id)), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), left_(left), right_(right), is_key_frame_(false)
{

}

Frame::~Frame()
{

}
= default;

Frame::Ptr Frame::createFrame()
{
static long factory_id = 0;
return Frame::Ptr( new Frame(factory_id++) );
return std::make_shared<Frame>(factory_id++);
}

double Frame::findDepth ( const cv::KeyPoint& kp )
Expand Down
55 changes: 26 additions & 29 deletions src/track.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,9 +130,7 @@ namespace StereoVO
}

Track::~Track()
{

}
= default;

bool Track::addFrame ( Frame::Ptr frame )
{
Expand Down Expand Up @@ -162,16 +160,16 @@ namespace StereoVO
matchCurr();
// ComputeStereoMatches();
featureMatching();
if( poseEstimationPnP() == true)
if(poseEstimationPnP())
{
if ( checkEstimatedPose() == true ) // a good estimation
if (checkEstimatedPose()) // 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
if (checkKeyFrame()) // is a key-frame
{
t.tic();
addKeyFrame();
Expand Down Expand Up @@ -311,21 +309,21 @@ namespace StereoVO
// TODO not very good
void Track::ComputeStereoMatches()
{
int N = keypoints_curr_left_.size();
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);
int N = static_cast<int>(keypoints_curr_left_.size());
mvuRight = vector<float>(static_cast<unsigned long>(N), -1.0f);
mvDepth = vector<float>(static_cast<unsigned long>(N), -1.0f);

const int thOrbDist = 75;

const int nRows = curr_->left_.rows;

//Assign keypoints to row table
vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());
vector<vector<size_t> > vRowIndices((unsigned long) nRows, vector<size_t>());

for(int i=0; i<nRows; i++)
vRowIndices[i].reserve(200);

const int Nr = keypoints_curr_right_.size();
const int Nr = static_cast<const int>(keypoints_curr_right_.size());

for(int iR=0; iR<Nr; iR++)
{
Expand Down Expand Up @@ -404,11 +402,11 @@ namespace StereoVO
if(disparity<=0)
{
disparity=0.01;
bestIdxR = uL-0.01;
bestIdxR = static_cast<size_t>(uL - 0.01);
}
mvDepth[iL]=curr_->camera_->bf_/disparity;
mvuRight[iL] = bestIdxR;
vDistIdx.push_back(pair<int,int>(bestDist,iL));
vDistIdx.emplace_back(bestDist,iL);
}
}

Expand All @@ -417,7 +415,7 @@ namespace StereoVO
const float thDist = median;

// int num = 0;
for(int i=vDistIdx.size()-1;i>=0;i--)
for(int i= static_cast<int>(vDistIdx.size() - 1); i >= 0; i--)
{
if(vDistIdx[i].first<thDist)
{
Expand All @@ -436,7 +434,7 @@ namespace StereoVO
{
cv::DMatch x;
x.queryIdx = i.second;
x.trainIdx = mvuRight[i.second];
x.trainIdx = static_cast<int>(mvuRight[i.second]);
matches_curr_good_.push_back(x);
}
}
Expand All @@ -452,16 +450,16 @@ 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);
int N = static_cast<int>(keypoints_curr_left_.size());
mvuRight = vector<float>(static_cast<unsigned long>(N), -1.0f);
mvDepth = vector<float>(static_cast<unsigned long>(N), -1.0f);

matcher_->knnMatch(descriptors_curr_left_, descriptors_curr_right_, matches_curr_, 2);



vector<pair<int, int> > vDistIdx;
vDistIdx.reserve(N);
vDistIdx.reserve(static_cast<unsigned long>(N));
const float minZ = curr_->camera_->b_;
const float minD = 2;
const float maxD = curr_->camera_->bf_ / minZ;
Expand All @@ -483,11 +481,12 @@ namespace StereoVO
if(disparity<=0)
{
disparity=0.01;
keypoints_curr_right_[ m[0].trainIdx ].pt.x = keypoints_curr_left_[ m[0].queryIdx ].pt.x-0.01;
keypoints_curr_right_[ m[0].trainIdx ].pt.x = static_cast<float>(
keypoints_curr_left_[ m[0].queryIdx ].pt.x - 0.01);
}
mvDepth[iL]=curr_->camera_->bf_/disparity;
mvuRight[iL] = keypoints_curr_right_[ m[0].trainIdx ].pt.x;
vDistIdx.push_back(pair<int,int>(m[0].distance,iL));
vDistIdx.emplace_back(m[0].distance,iL);
}

matches_curr_good_.push_back(m[0]);
Expand Down Expand Up @@ -554,8 +553,8 @@ namespace StereoVO
double x_e = 0,y_e = 0,e_i = 0;
for ( auto& m:matches )
{
if(m[0].distance > max_dis ) max_dis = m[0].distance;
if(m[0].distance < min_dis ) min_dis = m[0].distance;
if(m[0].distance > max_dis ) max_dis = static_cast<int>(m[0].distance);
if(m[0].distance < min_dis ) min_dis = static_cast<int>(m[0].distance);
double perfect = m[0].distance / m[1].distance;
if(perfect < 0.7 )
{
Expand Down Expand Up @@ -654,7 +653,7 @@ namespace StereoVO

for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
unsigned int v = static_cast<unsigned int>(*pa ^ *pb);
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
Expand All @@ -675,7 +674,7 @@ namespace StereoVO
pts2d.push_back ( keypoints_curr_left_[index].pt );
// pts2d_ref.push_back (keypoints_ref_left_[index].pt);
}
for ( MapPoint::Ptr pt:match_3dpts_ )
for (const MapPoint::Ptr &pt:match_3dpts_ )
{
pts3d.push_back( pt->pos_ /*getPositionCV()*/ );
}
Expand Down Expand Up @@ -825,9 +824,7 @@ namespace StereoVO
cv::vconcat(r_r_c, t_r_c, d);


if ( cv::norm(r_r_c) >key_frame_min_rot || cv::norm(t_r_c) >key_frame_min_trans )
return true;
return false;
return cv::norm(r_r_c) > key_frame_min_rot || cv::norm(t_r_c) > key_frame_min_trans;
}

double Track::my_nom(Mat src)
Expand Down Expand Up @@ -866,7 +863,7 @@ namespace StereoVO
iter = map_->map_points_.erase(iter);
continue;
}
if ( iter->second->good_ == false )
if (!iter->second->good_)
{
// TODO try triangulate this map point
// map_->map_points_
Expand Down

0 comments on commit 05e4391

Please sign in to comment.