public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

deactivate g2o in cmake

Browse Source
This commit is contained in:
gfuse
2021-09-20 14:23:00 +02:00
parent 31761fe5de
commit b848027f66
14 changed files with 891 additions and 10303 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+5 -5
View File
@@ -28,7 +28,7 @@ FIND_PACKAGE(OpenCV 4 REQUIRED)
FIND_PACKAGE(Eigen REQUIRED)
FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
FIND_PACKAGE(OpenCL REQUIRED)
FIND_PACKAGE(G2O REQUIRED)
#FIND_PACKAGE(G2O REQUIRED)
FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation)
@@ -53,8 +53,8 @@ INCLUDE_DIRECTORIES(
${Boost_INCLUDE_DIRS}
${OpenCL_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${G2O_INCLUDE_DIR}
/usr/include/suitesparse # for cholmod
#${G2O_INCLUDE_DIR}
#/usr/include/suitesparse # for cholmod
)
# Set link libraries
@@ -63,8 +63,8 @@ LIST(APPEND LINK_LIBS
${Boost_LIBRARIES}
${OpenCL_LIBRARIES}
${catkin_LIBRARIES}
${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE}
${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
# ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE}
# ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
)
file(GLOB SRC CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp")
GPU_version/vio/depth_filter_log.txt
+115 -9427
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+726 -836
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/cl_class.h
+14 -3
View File
@@ -16,7 +16,11 @@
class kernel{
public:
kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
kernel(cl::Program* program,std::string name){
cl_int error;
_kernel = new cl::Kernel(*program,name.c_str(),&error);
assert(error== CL_SUCCESS);
};
~kernel(){
for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
@@ -150,7 +154,10 @@ public:
cl_int run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) {
cl_int err=0;
cl::Event event;
queue->flush();
if(queue->flush()==CL_OUT_OF_HOST_MEMORY){
std::cerr<<"GPU out of memory goodbye:)\n";
exit(0);
}
if(z>1 && y>1){
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event);
}else if(z<2 && y>1){
@@ -160,7 +167,11 @@ public:
};
if(err!=CL_SUCCESS)return -1;
event.wait();
return queue->finish();
if(queue->finish()==CL_OUT_OF_HOST_MEMORY){
std::cerr<<"GPU out of memory goodbye:)\n";
exit(0);
}
return 1;
}
template<typename T>
GPU_version/vio/include/vio/nlls_solver.h
+1 -1
View File
@@ -70,7 +70,7 @@ protected:
/// Solve the linear system H*x = Jres. This function must set the update
/// step in the member variable x_. Must return true if the system could be
/// solved and false if it was singular.
virtual int
virtual bool
solve () = 0;
virtual void
GPU_version/vio/include/vio/nlls_solver_impl.hpp
+5 -1
View File
@@ -33,13 +33,15 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
rho_ = 0;
n_meas_ = 0;
double new_chi2 = computeResiduals(true, false);
std::cerr<<"36\t";
// solve the linear system
if(!solve())
{
// matrix was singular and could not be computed
if(verbose_)std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
stop_ = true;
}
std::cerr<<"43\t";
// check if error increased since last optimization
if((iter_ > 0 && new_chi2 > chi2_) || stop_)
{
@@ -52,8 +54,10 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
}
break;
}
std::cerr<<"57\t";
// update the model
update();
std::cerr<<"60\n";
chi2_ = new_chi2;
if(verbose_)
{
GPU_version/vio/include/vio/sparse_img_align.h
+1 -1
View File
@@ -73,7 +73,7 @@ protected:
void precomputeReferencePatches();
virtual double computeResiduals(const SE2& model, bool linearize_system, bool compute_weight_scale = false);
virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false){};
virtual int solve();
virtual bool solve();
virtual void update (const ModelType& old_model, ModelType& new_model);
virtual void update(){};
virtual void startIteration();
GPU_version/vio/include/vio/sparse_img_align_gpu.h
+1 -1
View File
@@ -73,7 +73,7 @@ protected:
return 0.0;
};
virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false);
virtual int solve();
virtual bool solve();
virtual void update();
virtual void update(const ModelType& old_model, ModelType& new_model) { };
virtual void startIteration();
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -9,7 +9,7 @@ svo:
structureoptim_max_pts: 50
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 30
init_min_inliers: 20
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
GPU_version/vio/src/frame_handler_mono.cpp
+2 -1
View File
@@ -162,8 +162,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
new_frame_->setKeyframe();
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
frame_utils::getSceneDepth(*klt_homography_init_->frame_ref_, depth_mean, depth_min);
depth_filter_->addKeyframe(klt_homography_init_->frame_ref_, depth_mean, 0.5*depth_min);
// add frame to map
map_.addKeyframe(new_frame_);
stage_ = STAGE_DEFAULT_FRAME;
GPU_version/vio/src/map.cpp
+1 -1
View File
@@ -222,7 +222,7 @@ MapPointCandidates::~MapPointCandidates()
void MapPointCandidates::newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2)
{
point->type_ = Point::TYPE_CANDIDATE;
boost::unique_lock<boost::mutex> lock(mut_);
//boost::unique_lock<boost::mutex> lock(mut_);
candidates_.push_back(PointCandidate(point, point->obs_.front()));
}
GPU_version/vio/src/sparse_img_align.cpp
+1 -1
View File
@@ -235,7 +235,7 @@ double SparseImgAlign::computeResiduals(
return chi2/n_meas_;
}
int SparseImgAlign::solve()
bool SparseImgAlign::solve()
{
x_ = H_.ldlt().solve(Jres_);
if((bool) std::isnan((double) x_[0]))
GPU_version/vio/src/sparse_img_align_gpu.cpp
+15 -22
View File
@@ -106,40 +106,33 @@ double SparseImgAlignGpu::computeResiduals(
return chi[0]/(feature_counter_*8);
}
int SparseImgAlignGpu::solve()
bool SparseImgAlignGpu::solve()
{
cl_float H[300*9]={0.0};
cl_float3 J[300]={0.0};
residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,9,feature_counter_,J);
for(int i = 1; i < feature_counter_; ++i){
H[0] += H[i*9];
H[1] += H[i*9+1];
H[2] += H[i*9+2];
H[3] += H[i*9+3];
H[4] += H[i*9+4];
H[5] += H[i*9+5];
H[6] += H[i*9+6];
H[7] += H[i*9+7];
H[8] += H[i*9+8];
J[0].x -= J[i].x;
J[0].y -= J[i].y;
J[0].z -= J[i].z;
H[0] += std::isnan(H[i*9]) ? 0.0 : H[i*9] ;
H[1] += std::isnan(H[i*9+1]) ? 0.0 : H[i*9+1];
H[2] += std::isnan(H[i*9+2]) ? 0.0 : H[i*9+2];
H[3] += std::isnan(H[i*9+3]) ? 0.0 : H[i*9+3];
H[4] += std::isnan(H[i*9+4]) ? 0.0 : H[i*9+4];
H[5] += std::isnan(H[i*9+5]) ? 0.0 : H[i*9+5];
H[6] += std::isnan(H[i*9+6]) ? 0.0 : H[i*9+6];
H[7] += std::isnan(H[i*9+7]) ? 0.0 : H[i*9+7];
H[8] += std::isnan(H[i*9+8]) ? 0.0 : H[i*9+8];
J[0].x -= std::isnan(J[i].x) ? 0.0 : J[i].x;
J[0].y -= std::isnan(J[i].y) ? 0.0 : J[i].y;
J[0].z -= std::isnan(J[i].z) ? 0.0 : J[i].z;
}
double Hd[9]={(double)H[0],(double)H[1],(double)H[2],
(double)H[3],(double)H[4],(double)H[5],
(double)H[6],(double)H[7],(double)H[8]};
double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
for(int i=0;i<9;++i){
if(i==0)std::cerr<<'\n'<<(double)J[0].x<<","<<(double)J[0].y<<","<<(double)J[0].z<<'\t';
std::cerr<<Hd[i]<<",";
}
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
return 0;
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z));
double norm=x_.norm();
if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
return 1;
return true;
}
void SparseImgAlignGpu::update()
{
GPU_version/vio/src/vo_node.cpp
+3 -2
View File
@@ -41,9 +41,10 @@ public:
bool start(vio::start::Request& req, vio::start::Response& res){
if(req.on==1 && !start_){
start_=true;
if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME)
if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME) {
vo_->depthFilter()->startThread();
vo_->start();
else{
}else{
vo_->depthFilter()->startThread();
vo_->reset();
}