Skip to content

Commit

Permalink
ch8/directmethod
Browse files Browse the repository at this point in the history
  • Loading branch information
xiang.gao committed Sep 6, 2016
1 parent 7bdf799 commit 3c47d3f
Show file tree
Hide file tree
Showing 7 changed files with 163 additions and 261 deletions.
2 changes: 2 additions & 0 deletions ch8/LKFlow/useLK.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ int main( int argc, char** argv )
last_color = color;
continue;
}
if ( color.data==nullptr || depth.data==nullptr )
continue;
// 对其他帧用LK跟踪特征点
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;
Expand Down
Binary file added ch8/data/data.tar.gz
View file
Binary file not shown.
4 changes: 1 addition & 3 deletions ch8/directMethod/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,9 @@ include_directories( ${G2O_INCLUDE_DIRS} )

include_directories( "/usr/include/eigen3" )

add_library( g2o_types_direct g2o_types.cpp )

set( G2O_LIBS
g2o_core g2o_types_sba g2o_solver_csparse g2o_stuff g2o_csparse_extension
)

add_executable( direct_sparse direct_sparse.cpp )
target_link_libraries( direct_sparse g2o_types_direct ${OpenCV_LIBS} ${G2O_LIBS} )
target_link_libraries( direct_sparse ${OpenCV_LIBS} ${G2O_LIBS} )
59 changes: 0 additions & 59 deletions ch8/directMethod/Untitled0.ipynb
View file

This file was deleted.

217 changes: 160 additions & 57 deletions ch8/directMethod/direct_sparse.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,20 +11,20 @@
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "g2o_types.h"
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;
using namespace g2o;

// 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
struct Measurement
{
Measurement( Eigen::Vector3d p, float g ): pos_world(p), grayscale(g) {}
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
Eigen::Vector3d pos_world;
float grayscale;
};
Expand All @@ -37,17 +37,118 @@ inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy,
return Eigen::Vector3d ( xx, yy, zz );
}

inline Eigen::Vector2d project3Dto2D( float x, float y, float z, float fx, float fy, float cx, float cy )
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
float u = fx*x/z+cx;
float v = fy*y/z+cy;
return Eigen::Vector2d(u,v);
return Eigen::Vector2d ( u,v );
}

// 直接法估计位姿
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
bool poseEstimationDirect( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );


// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

EdgeSE3ProjectDirect() {}

EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
{}

virtual void computeError()
{
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
float x = x_local[0]*fx_/x_local[2] + cx_;
float y = x_local[1]*fy_/x_local[2] + cy_;
// check x,y is in the image
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
{
_error ( 0,0 ) = 0.0;
this->setLevel ( 1 );
}
else
{
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
}
}

// plus in manifold
virtual void linearizeOplus( )
{
if ( level() == 1 )
{
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
return;
}
VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book

double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;

float u = x*fx_*invz + cx_;
float v = y*fy_*invz + cy_;

// jacobian from se3 to u,v
// NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;

jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}

// dummy read and write functions because we don't care...
virtual bool read ( std::istream& in ) {}
virtual bool write ( std::ostream& out ) const {}

protected:
// get a gray scale value from reference image (bilinear interpolated)
inline float getPixelValue ( float x, float y )
{
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
float xx = x - floor ( x );
float yy = y - floor ( y );
return float (
( 1-xx ) * ( 1-yy ) * data[0] +
xx* ( 1-yy ) * data[1] +
( 1-xx ) *yy*data[ image_->step ] +
xx*yy*data[image_->step+1]
);
}
public:
Eigen::Vector3d x_world_; // 3D point in world frame
float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
cv::Mat* image_=nullptr; // reference image
};

int main ( int argc, char** argv )
{
Expand All @@ -56,7 +157,7 @@ int main ( int argc, char** argv )
cout<<"usage: useLK path_to_dataset"<<endl;
return 1;
}
srand( (unsigned int) time(0) );
srand ( ( unsigned int ) time ( 0 ) );
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";

Expand All @@ -74,16 +175,18 @@ int main ( int argc, char** argv )
Eigen::Matrix3f K;
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

cv::Mat prev_color;
// 我们演示两个图像间的直接法计算
for ( int index=0; index<5; index++ )
// 我们以第一个图像为参考，对后续图像和参考图像做直接法
for ( int index=0; index<10; index++ )
{
cout<<"*********** loop "<<index<<" ************"<<endl;
fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
color = cv::imread ( path_to_dataset+"/"+rgb_file );
depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );
if ( color.data==nullptr || depth.data==nullptr )
continue;
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
if ( index ==0 )
{
Expand All @@ -94,49 +197,49 @@ int main ( int argc, char** argv )
for ( auto kp:keypoints )
{
// 去掉邻近边缘处的点
if ( kp.pt.x < 20 || kp.pt.y < 20 || (kp.pt.x+20)>color.cols || (kp.pt.y+20)>color.rows )
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
continue;
ushort d = depth.ptr<ushort> ( int( kp.pt.y ) ) [ int(kp.pt.x) ];
ushort d = depth.ptr<ushort> ( int ( kp.pt.y ) ) [ int ( kp.pt.x ) ];
if ( d==0 )
continue;
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
float grayscale = float ( gray.ptr<uchar> ( int(kp.pt.y) ) [ int(kp.pt.x) ] );
measurements.push_back ( Measurement( p3d, grayscale ) );
float grayscale = float ( gray.ptr<uchar> ( int ( kp.pt.y ) ) [ int ( kp.pt.x ) ] );
measurements.push_back ( Measurement ( p3d, grayscale ) );
}
prev_color = color.clone();
continue;
}
// 使用直接法计算相机运动及投影点
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
poseEstimationDirect( measurements, &gray, K, Tcw );
poseEstimationDirect ( measurements, &gray, K, Tcw );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"direct method costs time: "<<time_used.count()<<" seconds."<<endl;
cout<<"Tcw="<<Tcw.matrix()<<endl;
// plot the feature points
cv::Mat img_show( color.rows*2, color.cols, CV_8UC3 );
prev_color.copyTo( img_show( cv::Rect(0,0,color.cols, color.rows) ) );
color.copyTo( img_show(cv::Rect(0,color.rows,color.cols, color.rows)) );
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
cout<<"Tcw="<<Tcw.matrix() <<endl;

// plot the feature points
cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
for ( Measurement m:measurements )
{
if (rand() > RAND_MAX/5)
if ( rand() > RAND_MAX/5 )
continue;
Eigen::Vector3d p = m.pos_world;
Eigen::Vector2d pixel_prev = project3Dto2D( p(0,0), p(1,0), p(2,0), fx, fy, cx, cy );
Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
Eigen::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D( p2(0,0), p2(1,0), p2(2,0), fx, fy, cx, cy );
float b = 255*float(rand())/RAND_MAX;
float g = 255*float(rand())/RAND_MAX;
float r = 255*float(rand())/RAND_MAX;
cv::circle(img_show, cv::Point2d(pixel_prev(0,0), pixel_prev(1,0)), 8, cv::Scalar(b,g,r), 2 );
cv::circle(img_show, cv::Point2d(pixel_now(0,0), pixel_now(1,0)+color.rows), 8, cv::Scalar(b,g,r), 2 );
cv::line( img_show, cv::Point2d(pixel_prev(0,0), pixel_prev(1,0)), cv::Point2d(pixel_now(0,0), pixel_now(1,0)+color.rows), cv::Scalar(b,g,r), 1 );
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );

float b = 255*float ( rand() ) /RAND_MAX;
float g = 255*float ( rand() ) /RAND_MAX;
float r = 255*float ( rand() ) /RAND_MAX;
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
}
cv::imshow( "result", img_show );
cv::waitKey(0);
cv::imshow ( "result", img_show );
cv::waitKey ( 0 );

}
return 0;
}
Expand All @@ -146,35 +249,35 @@ bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat*
// 初始化g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
DirectBlock* solver_ptr = new DirectBlock( linearSolver );
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm( solver );
optimizer.setVerbose( true ); // 打开调试输出
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setEstimate( g2o::SE3Quat(Tcw.rotation(), Tcw.translation()) );
pose->setId(0);
optimizer.addVertex( pose );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );

g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
pose->setId ( 0 );
optimizer.addVertex ( pose );

// 添加边
int id=1;
for( Measurement m: measurements )
for ( Measurement m: measurements )
{
g2o::EdgeSE3ProjectDirect* edge = new g2o::EdgeSE3ProjectDirect(
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
K(0,0), K(1,1), K(0,2), K(1,2), gray
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
);
edge->setVertex( 0, pose );
edge->setMeasurement( m.grayscale );
edge->setInformation( Eigen::Matrix<double,1,1>::Identity() );
edge->setId( id++ );
optimizer.addEdge(edge);
edge->setVertex ( 0, pose );
edge->setMeasurement ( m.grayscale );
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
edge->setId ( id++ );
optimizer.addEdge ( edge );
}
cout<<"edges in graph: "<<optimizer.edges().size()<<endl;
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
optimizer.initializeOptimization();
optimizer.optimize(30);
optimizer.optimize ( 30 );
Tcw = pose->estimate();
}

Loading

0 comments on commit 3c47d3f

Please sign in to comment.