g2o版本问题
0、g2o版本问题
start optimization terminate called after throwing an instance of ‘std::bad_alloc
由于高博SLAM十四讲中使用的g2o版本年限差距
而且g2o版本的更迭也比较快，
所以将版本换成高翔博士的版本
cd g2o
mkdir build
cmake ..
make -j12
sudo make install
orb-slam2和orb-slam3等一些开源方案一般会自己提供第三方库，将其编译为链接库
比如g2o编译为libg2o.so
高博的代码是寻找系统安装的g2o
如果以后想切换
cd g2o_原先文件夹
cd build
sudo make install
当然也可以自己修改高博代码中的CMakeLists.txt的编译链接关系，仿照orb-slam2/3将3rdParty中的g2o编译为libg2o.so
系统中usr保持为自己想要的版本（比如很新的）
可以，但没必要
怕麻烦来回切换就行
一、ch13 demo运行
参考：
https://blog.csdn.net/jiachang98/article/details/121700288.
1.1 一些报错参考1
https://blog.csdn.net/Evanzxh/article/details/119757742.
1.2 一些报错参考2-fmt
Sophus链接后面加上fmt就行
https://blog.csdn.net/weixin_53660567/article/details/120074124.
https://blog.csdn.net/Evanzxh/article/details/119757742.
c++14标准问题
https://blog.csdn.net/weixin_43979073/article/details/119880970.
1.3 运行效果
cd bin
./run_kitti_stereo
注意 配置文件 …/config/default.yaml的路径格式设置正确
三、opencv版本问题
https://www.cnblogs.com/cc111/p/9466463.html
https://blog.csdn.net/weixin_43979073/article/details/119880970
direct_method.cpp编译问题参见
https://github.com/gaoxiang12/slambook2/issues/32.
3.1 编译问题
3.2 运行结果
3.3 解决
参照https://github.com/gaoxiang12/slambook2/issues/32.修改代码
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace cv;
string file_1 = "./LK1.png"; // first image
string file_2 = "./LK2.png"; // second image
/**
* single level optical flow
* @param [in] img1 the first image
* @param [in] img2 the second image
* @param [in] kp1 keypoints in img1
* @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1
* @param [out] success true if a keypoint is tracked successfully
* @param [in] inverse use inverse formulation?
*/
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse = false,
bool has_initial_guess = false);
/**
* multi level optical flow, scale of pyramid is set to 2 by default
* the image pyramid will be create inside the function
* @param [in] img1 the first pyramid
* @param [in] img2 the second pyramid
* @param [in] kp1 keypoints in img1
* @param [out] kp2 keypoints in img2
* @param [out] success true if a keypoint is tracked successfully
* @param [in] inverse set true to enable inverse formulation
*/
void OpticalFlowMultiLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse = false);
/**
* get a gray scale value from reference image (bi-linear interpolated)
* @param img
* @param x
* @param y
* @return the interpolated value of this pixel
*/
inline float GetPixelValue(const cv::Mat &img, float x, float y)
{
// boundary check
if (x < 0)
x = 0;
if (y < 0)
y = 0;
if (x >= img.cols - 1)
x = img.cols - 2;
if (y >= img.rows - 1)
y = img.rows - 2;
float xx = x - floor(x);
float yy = y - floor(y);
int x_a1 = std::min(img.cols - 1, int(x) + 1);
int y_a1 = std::min(img.rows - 1, int(y) + 1);
return (1 - xx) * (1 - yy) * img.at<uchar>(y, x) + xx * (1 - yy) * img.at<uchar>(y, x_a1) + (1 - xx) * yy * img.at<uchar>(y_a1, x) + xx * yy * img.at<uchar>(y_a1, x_a1);
}
/// Optical flow tracker and interface
class OpticalFlowTracker : public cv::ParallelLoopBody
{
public:
OpticalFlowTracker(
const Mat &img1_,
const Mat &img2_,
const vector<KeyPoint> &kp1_,
vector<KeyPoint> &kp2_,
vector<bool> &success_,
bool inverse_ = true, bool has_initial_ = false) : img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
has_initial(has_initial_) {}
//void calculateOpticalFlow(const Range &range);
virtual void operator()(const cv::Range &range) const
{
// parameters
int half_patch_size = 4;
int iterations = 10;
for (size_t i = range.start; i < range.end; i++)
{
auto kp = kp1[i];
double dx = 0, dy = 0; // dx,dy need to be estimated
if (has_initial)
{
dx = kp2[i].pt.x - kp.pt.x;
dy = kp2[i].pt.y - kp.pt.y;
}
double cost = 0, lastCost = 0;
bool succ = true; // indicate if this point succeeded
// Gauss-Newton iterations
Eigen::Matrix2d H = Eigen::Matrix2d::Zero(); // hessian
Eigen::Vector2d b = Eigen::Vector2d::Zero(); // bias
Eigen::Vector2d J; // jacobian
for (int iter = 0; iter < iterations; iter++)
{
if (inverse == false)
{
H = Eigen::Matrix2d::Zero();
b = Eigen::Vector2d::Zero();
}
else
{
// only reset b
b = Eigen::Vector2d::Zero();
}
cost = 0;
// compute cost and jacobian
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++)
{
double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);
; // Jacobian
if (inverse == false)
{
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1)));
}
else if (iter == 0)
{
// in inverse mode, J keeps same for all iterations
// NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
J = -1.0 * Eigen::Vector2d(
0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1)));
}
// compute H, b and set cost;
b += -error * J;
cost += error * error;
if (inverse == false || iter == 0)
{
// also update H
H += J * J.transpose();
}
}
// compute update
Eigen::Vector2d update = H.ldlt().solve(b);
if (std::isnan(update[0]))
{
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
succ = false;
break;
}
if (iter > 0 && cost > lastCost)
{
break;
}
// update dx, dy
dx += update[0];
dy += update[1];
lastCost = cost;
succ = true;
if (update.norm() < 1e-2)
{
// converge
break;
}
}
success[i] = succ;
// set kp2
kp2[i].pt = kp.pt + Point2f(dx, dy);
}
}
private:
const Mat &img1;
const Mat &img2;
const vector<KeyPoint> &kp1;
vector<KeyPoint> &kp2;
vector<bool> &success;
bool inverse = true;
bool has_initial = false;
};
int main(int argc, char **argv)
{
// images, note they are CV_8UC1, not CV_8UC3
Mat img1 = imread(file_1, 0);
Mat img2 = imread(file_2, 0);
// key points, using GFTT here.
vector<KeyPoint> kp1;
Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
detector->detect(img1, kp1);
// now lets track these key points in the second image
// first use single level LK in the validation picture
vector<KeyPoint> kp2_single;
vector<bool> success_single;
OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);
// then test multi-level LK
vector<KeyPoint> kp2_multi;
vector<bool> success_multi;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optical flow by gauss-newton: " << time_used.count() << endl;
// use opencv's flow for validation
vector<Point2f> pt1, pt2;
for (auto &kp : kp1)
pt1.push_back(kp.pt);
vector<uchar> status;
vector<float> error;
t1 = chrono::steady_clock::now();
cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
t2 = chrono::steady_clock::now();
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optical flow by opencv: " << time_used.count() << endl;
// plot the differences of those functions
Mat img2_single;
cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
for (int i = 0; i < kp2_single.size(); i++)
{
if (success_single[i])
{
cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
}
}
Mat img2_multi;
cv::cvtColor(img2, img2_multi, CV_GRAY2BGR);
for (int i = 0; i < kp2_multi.size(); i++)
{
if (success_multi[i])
{
cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
}
}
Mat img2_CV;
cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
for (int i = 0; i < pt2.size(); i++)
{
if (status[i])
{
cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
}
}
cv::imshow("tracked single level", img2_single);
cv::imshow("tracked multi level", img2_multi);
cv::imshow("tracked by opencv", img2_CV);
cv::waitKey(0);
return 0;
}
void OpticalFlowSingleLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse, bool has_initial)
{
kp2.resize(kp1.size());
success.resize(kp1.size());
OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
parallel_for_(Range(0, kp1.size()), tracker);
}
void OpticalFlowMultiLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse)
{
// parameters
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {1.0, 0.5, 0.25, 0.125};
// create pyramids
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
vector<Mat> pyr1, pyr2; // image pyramids
for (int i = 0; i < pyramids; i++)
{
if (i == 0)
{
pyr1.push_back(img1);
pyr2.push_back(img2);
}
else
{
Mat img1_pyr, img2_pyr;
cv::resize(pyr1[i - 1], img1_pyr,
cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
cv::resize(pyr2[i - 1], img2_pyr,
cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
pyr1.push_back(img1_pyr);
pyr2.push_back(img2_pyr);
}
}
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "build pyramid time: " << time_used.count() << endl;
// coarse-to-fine LK tracking in pyramids
vector<KeyPoint> kp1_pyr, kp2_pyr;
for (auto &kp : kp1)
{
auto kp_top = kp;
kp_top.pt *= scales[pyramids - 1];
kp1_pyr.push_back(kp_top);
kp2_pyr.push_back(kp_top);
}
for (int level = pyramids - 1; level >= 0; level--)
{
// from coarse to fine
success.clear();
t1 = chrono::steady_clock::now();
OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);
t2 = chrono::steady_clock::now();
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "track pyr " << level << " cost time: " << time_used.count() << endl;
if (level > 0)
{
for (auto &kp : kp1_pyr)
kp.pt /= pyramid_scale;
for (auto &kp : kp2_pyr)
kp.pt /= pyramid_scale;
}
}
for (auto &kp : kp2_pyr)
kp2.push_back(kp);
}
3.4 运行结果
三、g2o_viewer找不到命令
安装依赖
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev
sudo apt-get install qtdeclarative5-dev
sudo apt-get install qt5-qmake
sudo apt-get install libqglviewer-dev
sudo apt-get install libqglviewer-dev-qt5
重新编译g2o（使用高博支持版本）
cmake …
make -j4
sudo make install
运行
cd g2o/bin
./g2o_viewer /path/xxx.g2o
四、错误make[2]: *** 没有规则可制作目标“/usr/local/lib/libDBoW3.a”
4.1 按照高博github安装DBoW3
mkdir build
cd build
cmake ..
make -j12
sudo make install
4.2 将静态库改为动态库
cmake_minimum_required( VERSION 2.8 )
project( loop_closure )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
# opencv
find_package( OpenCV 3.1 REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
# dbow3
# dbow3 is a simple lib so I assume you installed it in default directory
set( DBoW3_INCLUDE_DIRS "/usr/local/include" )
set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" ) #这里.a改为.so
add_executable( feature_training feature_training.cpp )
target_link_libraries( feature_training ${OpenCV_LIBS} ${DBoW3_LIBS} )
add_executable( loop_closure loop_closure.cpp )
target_link_libraries( loop_closure ${OpenCV_LIBS} ${DBoW3_LIBS} )
add_executable( gen_vocab gen_vocab_large.cpp )
target_link_libraries( gen_vocab ${OpenCV_LIBS} ${DBoW3_LIBS} )
4.3 运行
在ch11目录下
~/shenlan_guyue_ros_cource/slambook2/ch11$ ./build/feature_training
4.4 运行 gen_vocab溢出
./build/gen_vocab /home/user/ORB_SLAM2_AddSemantic/my_initial_dataset/TUM/rgbd_dataset_freiburg3_walking_xyz
terminate called after throwing an instance of 'std::out_of_range'
what(): basic_string::substr: __pos (which is 140) > this->size() (which is 0)
已放弃 (核心已转储)
代码做如下更改
if(index>827)
{
std::cout<<"hello=index="<<index<<std::endl;
break;
}
827是图片个数
读完让他退出
#include "DBoW3/DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>
using namespace cv;
using namespace std;
int main( int argc, char** argv )
{
string dataset_dir = argv[1];
ifstream fin ( dataset_dir+"/associate.txt" );
if ( !fin )
{
cout<<"please generate the associate file called associate.txt!"<<endl;
return 1;
}
vector<string> rgb_files, depth_files;
vector<double> rgb_times, depth_times;
while ( !fin.eof() )
{
string rgb_time, rgb_file, depth_time, depth_file;
fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
rgb_times.push_back ( atof ( rgb_time.c_str() ) );
depth_times.push_back ( atof ( depth_time.c_str() ) );
rgb_files.push_back ( dataset_dir+"/"+rgb_file );
depth_files.push_back ( dataset_dir+"/"+depth_file );
if ( fin.good() == false )
break;
}
fin.close();
cout<<"generating features ... "<<endl;
vector<Mat> descriptors;
Ptr< Feature2D > detector = ORB::create();
int index = 1;
for ( string rgb_file:rgb_files )
{
Mat image = imread(rgb_file);
vector<KeyPoint> keypoints;
Mat descriptor;
detector->detectAndCompute( image, Mat(), keypoints, descriptor );
descriptors.push_back( descriptor );
cout<<"extracting features from image " << index++ <<endl;
if(index>827)
{
std::cout<<rgb_files.size()<<std::endl;
std::cout<<rgb_files[826]<<std::endl;
std::cout<<rgb_files[827]<<std::endl;
std::cout<<"hello=index="<<index<<std::endl;
break;
}
}
std::cout<<"hello=================================="<<std::endl;
cout<<"extract total "<<descriptors.size()*500<<" features."<<endl;
// create vocabulary
cout<<"creating vocabulary, please wait ... "<<endl;
DBoW3::Vocabulary vocab;
vocab.create( descriptors );
cout<<"vocabulary info: "<<vocab<<endl;
vocab.save( "vocab_larger.yml.gz" );
cout<<"done"<<endl;
return 0;
}
4.5 问题所在 ifstream.eof()函数
ifstream.eof()会读到文件末位，读到下一行
https://blog.csdn.net/wenfan0934/article/details/120552244
https://blog.csdn.net/Nine_CC/article/details/107166518
https://www.cnblogs.com/sineatos/p/4111348.html
https://blog.csdn.net/ke7in1314/article/details/28429283
故将代码做如下更改
#include "DBoW3/DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>
using namespace cv;
using namespace std;
string rgb_time, rgb_file, depth_time, depth_file;
int main( int argc, char** argv )
{
string dataset_dir = argv[1];
ifstream fin ( dataset_dir+"/associate.txt" );
if ( !fin )
{
cout<<"please generate the associate file called associate.txt!"<<endl;
return 1;
}
vector<string> rgb_files, depth_files;
vector<double> rgb_times, depth_times;
while ( !fin.eof() && fin>>rgb_time) //发现读不到东西退出
{
fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
rgb_times.push_back ( atof ( rgb_time.c_str() ) );
depth_times.push_back ( atof ( depth_time.c_str() ) );
rgb_files.push_back ( dataset_dir+"/"+rgb_file );
depth_files.push_back ( dataset_dir+"/"+depth_file );
// if ( fin.peek() == EOF )
// break;
}
fin.close();
cout<<"generating features ... "<<endl;
vector<Mat> descriptors;
Ptr< Feature2D > detector = ORB::create();
int index = 1;
for ( string rgb_file:rgb_files )
{
Mat image = imread(rgb_file);
vector<KeyPoint> keypoints;
Mat descriptor;
detector->detectAndCompute( image, Mat(), keypoints, descriptor );
descriptors.push_back( descriptor );
cout<<"extracting features from image " << index++ <<endl;
}
cout<<"extract total "<<descriptors.size()*500<<" features."<<endl;
// create vocabulary
cout<<"creating vocabulary, please wait ... "<<endl;
DBoW3::Vocabulary vocab;
vocab.create( descriptors );
cout<<"vocabulary info: "<<vocab<<endl;
vocab.save( "vocab_larger.yml.gz" );
cout<<"done"<<endl;
return 0;
}
1034
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
