第一版的代码：
direct_semidense.cpp
#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.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;
/********************************************
* 本节演示了RGBD上的半稠密直接法
********************************************/
// 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
struct Measurement
{
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
Eigen::Vector3d pos_world;
float grayscale;
};
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
float zz = float ( d ) /scale;
float xx = zz* ( x-cx ) /fx;
float yy = zz* ( y-cy ) /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 )
{
float u = fx*x/z+cx;
float v = fy*y/z+cy;
return Eigen::Vector2d ( u,v );
}
// 直接法估计位姿
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
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 )
{
if ( argc != 2 )
{
cout<<"usage: useLK path_to_dataset"<<endl;
return 1;
}
srand ( ( unsigned int ) time ( 0 ) );
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
ifstream fin ( associate_file );
string rgb_file, depth_file, time_rgb, time_depth;
cv::Mat color, depth, gray;
vector<Measurement> measurements;
// 相机内参
float cx = 325.5;
float cy = 253.5;
float fx = 518.0;
float fy = 519.0;
float depth_scale = 1000.0;
Eigen::Matrix3f K;
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
cv::Mat prev_color;
// 我们以第一个图像为参考，对后续图像和参考图像做直接法
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 )
{
// select the pixels with high gradiants
for ( int x=10; x<gray.cols-10; x++ )
for ( int y=10; y<gray.rows-10; y++ )
{
Eigen::Vector2d delta (
gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1],
gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
);
if ( delta.norm() < 50 )
continue;
ushort d = depth.ptr<ushort> (y)[x];
if ( d==0 )
continue;
Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
float grayscale = float ( gray.ptr<uchar> (y) [x] );
measurements.push_back ( Measurement ( p3d, grayscale ) );
}
prev_color = color.clone();
cout<<"add total "<<measurements.size()<<" measurements."<<endl;
continue;
}
// 使用直接法计算相机运动
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
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 ) ) );
for ( Measurement m:measurements )
{
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::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
continue;
float b = 0;
float g = 250;
float r = 0;
img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3] = b;
img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+1] = g;
img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+2] = r;
img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3] = b;
img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+1] = g;
img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+2] = r;
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 4, cv::Scalar ( b,g,r ), 2 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 4, cv::Scalar ( b,g,r ), 2 );
}
cv::imshow ( "result", img_show );
cv::waitKey ( 0 );
}
return 0;
}
bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
// 初始化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 );
//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
std::unique_ptr<DirectBlock::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>());
std::unique_ptr<DirectBlock> solver_ptr ( new DirectBlock ( std::move(linearSolver)));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
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 )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
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 );
}
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
optimizer.initializeOptimization();
optimizer.optimize ( 30 );
Tcw = pose->estimate();
}
CMakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( directMethod )
#set( CMAKE_BUILD_TYPE Release )
#set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
#set(CMAKE_CXX_FLAGS "-std=c++14 ${SSE_FLAGS} -g -O3 -march=native")
set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
# 添加cmake模块路径
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )
find_package( G2O )
include_directories( ${G2O_INCLUDE_DIRS} )
include_directories(
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
"/usr/include/eigen3/"
${Pangolin_INCLUDE_DIRS}
)
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 ${OpenCV_LIBS} ${G2O_LIBS})
add_executable( direct_sparse1 direct_sparse1.cpp )
target_link_libraries( direct_sparse1 ${OpenCV_LIBS} ${G2O_LIBS} )
add_executable( direct_semidense direct_semidense.cpp )
target_link_libraries( direct_semidense ${OpenCV_LIBS} ${G2O_LIBS} )
add_executable( direct_semidense1 direct_semidense1.cpp )
target_link_libraries( direct_semidense1 ${OpenCV_LIBS} ${G2O_LIBS} )
执行结果：
./direct_semidense ../../data*********** loop 0 ************
add total 12556 measurements.
*********** loop 1 ************
edges in graph: 12556
iteration= 0 chi2= 72633591.401419 time= 0.00203194 cumTime= 0.00203194 edges= 12556 schur= 0 lambda= 12900954.939934 levenbergIter= 1
iteration= 1 chi2= 62926900.131419 time= 0.00198175 cumTime= 0.00401369 edges= 12556 schur= 0 lambda= 4300318.313311 levenbergIter= 1
iteration= 2 chi2= 52845484.671121 time= 0.00190874 cumTime= 0.00592243 edges= 12556 schur= 0 lambda= 1433439.437770 levenbergIter= 1
iteration= 3 chi2= 45449292.558261 time= 0.00192286 cumTime= 0.00784528 edges= 12556 schur= 0 lambda= 477813.145923 levenbergIter= 1
iteration= 4 chi2= 38422805.114205 time= 0.00195591 cumTime= 0.00980119 edges= 12556 schur= 0 lambda= 159271.048641 levenbergIter= 1
iteration= 5 chi2= 31970398.890953 time= 0.0019295 cumTime= 0.0117307 edges= 12556 schur= 0 lambda= 53090.349547 levenbergIter= 1
iteration= 6 chi2= 24270565.530351 time= 0.00190734 cumTime= 0.013638 edges= 12556 schur= 0 lambda= 17696.783182 levenbergIter= 1
iteration= 7 chi2= 12153446.174612 time= 0.00197201 cumTime= 0.01561 edges= 12556 schur= 0 lambda= 5898.927727 levenbergIter= 1
iteration= 8 chi2= 5615434.148147 time= 0.0020526 cumTime= 0.0176626 edges= 12556 schur= 0 lambda= 1966.309242 levenbergIter= 1
iteration= 9 chi2= 4849512.586059 time= 0.00202361 cumTime= 0.0196862 edges= 12556 schur= 0 lambda= 655.436414 levenbergIter= 1
iteration= 10 chi2= 4785381.917957 time= 0.00190271 cumTime= 0.021589 edges= 12556 schur= 0 lambda= 218.478805 levenbergIter= 1
iteration= 11 chi2= 4785319.436062 time= 0.00190333 cumTime= 0.0234923 edges= 12556 schur= 0 lambda= 145.652536 levenbergIter= 1
iteration= 12 chi2= 4785319.043803 time= 0.00653219 cumTime= 0.0300245 edges= 12556 schur= 0 lambda= 1708231013067781.250000 levenbergIter= 10
direct method costs time: 0.0432346 seconds.
Tcw= 0.999626 0.0206545 -0.0179199 0.0159529
-0.0207425 0.999774 -0.00473951 -0.00352488
0.0178179 0.00510944 0.999828 0.0474896
0 0 0 1
*********** loop 2 ************
edges in graph: 12556
iteration= 0 chi2= 64054091.635612 time= 0.00207077 cumTime= 0.00207077 edges= 12556 schur= 0 lambda= 11746441.098549 levenbergIter= 1
iteration= 1 chi2= 49928789.420353 time= 0.00200024 cumTime= 0.00407101 edges= 12556 schur= 0 lambda= 3915480.366183 levenbergIter= 1
iteration= 2 chi2= 40905518.557446 time= 0.00192051 cumTime= 0.00599152 edges= 12556 schur= 0 lambda= 1305160.122061 levenbergIter= 1
iteration= 3 chi2= 30834251.286537 time= 0.00197487 cumTime= 0.0079664 edges= 12556 schur= 0 lambda= 435053.374020 levenbergIter= 1
iteration= 4 chi2= 21824701.679727 time= 0.00193962 cumTime= 0.00990602 edges= 12556 schur= 0 lambda= 145017.791340 levenbergIter= 1
iteration= 5 chi2= 9755306.166373 time= 0.00192242 cumTime= 0.0118284 edges= 12556 schur= 0 lambda= 48339.263780 levenbergIter= 1
iteration= 6 chi2= 7175144.162209 time= 0.00212803 cumTime= 0.0139565 edges= 12556 schur= 0 lambda= 16113.087927 levenbergIter= 1
iteration= 7 chi2= 6906428.680463 time= 0.0019498 cumTime= 0.0159063 edges= 12556 schur= 0 lambda= 5371.029309 levenbergIter= 1
iteration= 8 chi2= 6866111.966045 time= 0.00191189 cumTime= 0.0178182 edges= 12556 schur= 0 lambda= 1790.343103 levenbergIter= 1
iteration= 9 chi2= 6862465.198207 time= 0.00197429 cumTime= 0.0197925 edges= 12556 schur= 0 lambda= 1193.562069 levenbergIter= 1
iteration= 10 chi2= 6862411.175096 time= 0.00191108 cumTime= 0.0217035 edges= 12556 schur= 0 lambda= 795.708046 levenbergIter= 1
iteration= 11 chi2= 6862368.547439 time= 0.00563595 cumTime= 0.0273395 edges= 12556 schur= 0 lambda= 142397501404.643890 levenbergIter= 8
iteration= 12 chi2= 6862364.922391 time= 0.00299751 cumTime= 0.030337 edges= 12556 schur= 0 lambda= 759453340824.767334 levenbergIter= 3
iteration= 13 chi2= 6862363.644881 time= 0.00298225 cumTime= 0.0333192 edges= 12556 schur= 0 lambda= 4050417817732.092285 levenbergIter= 3
iteration= 14 chi2= 6862363.076746 time= 0.00356282 cumTime= 0.0368821 edges= 12556 schur= 0 lambda= 86408913444951.296875 levenbergIter= 4
iteration= 15 chi2= 6862361.783253 time= 0.00242732 cumTime= 0.0393094 edges= 12556 schur= 0 lambda= 57605942296634.195312 levenbergIter= 2
iteration= 16 chi2= 6862361.740822 time= 0.00414043 cumTime= 0.0434498 edges= 12556 schur= 0 lambda= 19662828303917804.000000 levenbergIter= 5
iteration= 17 chi2= 6862361.555867 time= 0.0019127 cumTime= 0.0453625 edges= 12556 schur= 0 lambda= 6554276101305934.000000 levenbergIter= 1
iteration= 18 chi2= 6862361.528706 time= 0.00298187 cumTime= 0.0483444 edges= 12556 schur= 0 lambda= 17478069603482490.000000 levenbergIter= 3
iteration= 19 chi2= 6862361.528658 time= 0.00351384 cumTime= 0.0518582 edges= 12556 schur= 0 lambda= 745730969748586240.000000 levenbergIter= 4
iteration= 20 chi2= 6862361.528658 time= 0.00199555 cumTime= 0.0538538 edges= 12556 schur= 0 lambda= 1491461939497172480.000000 levenbergIter= 1
direct method costs time: 0.0710072 seconds.
Tcw= 0.998905 0.0388895 -0.0259903 -0.0103611
-0.0389581 0.999239 -0.00213735 -0.0144453
0.0258874 0.00314754 0.99966 0.0861913
0 0 0 1
*********** loop 3 ************
edges in graph: 12556
iteration= 0 chi2= 82910879.482012 time= 0.00398495 cumTime= 0.00398495 edges= 12556 schur= 0 lambda= 6169406.099584 levenbergIter= 1
iteration= 1 chi2= 65771814.131560 time= 0.00324883 cumTime= 0.00723378 edges= 12556 schur= 0 lambda= 2056468.699861 levenbergIter= 1
iteration= 2 chi2= 50704891.266112 time= 0.00285622 cumTime= 0.01009 edges= 12556 schur= 0 lambda= 685489.566620 levenbergIter= 1
iteration= 3 chi2= 42573860.067371 time= 0.00267184 cumTime= 0.0127618 edges= 12556 schur= 0 lambda= 228496.522207 levenbergIter= 1
iteration= 4 chi2= 31836768.361608 time= 0.00253297 cumTime= 0.0152948 edges= 12556 schur= 0 lambda= 76165.507402 levenbergIter= 1
iteration= 5 chi2= 17969581.308843 time= 0.00234691 cumTime= 0.0176417 edges= 12556 schur= 0 lambda= 25388.502467 levenbergIter= 1
iteration= 6 chi2= 7931996.778542 time= 0.00222524 cumTime= 0.019867 edges= 12556 schur= 0 lambda= 8462.834156 levenbergIter= 1
iteration= 7 chi2= 7307169.083813 time= 0.00208339 cumTime= 0.0219504 edges= 12556 schur= 0 lambda= 2820.944719 levenbergIter= 1
iteration= 8 chi2= 7274251.441660 time= 0.00199832 cumTime= 0.0239487 edges= 12556 schur= 0 lambda= 1880.629812 levenbergIter= 1
iteration= 9 chi2= 7270720.342762 time= 0.00197917 cumTime= 0.0259278 edges= 12556 schur= 0 lambda= 626.876604 levenbergIter= 1
iteration= 10 chi2= 7270710.835258 time= 0.00610449 cumTime= 0.0320323 edges= 12556 schur= 0 lambda= 14359544071374.978516 levenbergIter= 9
iteration= 11 chi2= 7270693.258983 time= 0.00195883 cumTime= 0.0339912 edges= 12556 schur= 0 lambda= 6796651598741.770508 levenbergIter= 1
iteration= 12 chi2= 7270681.062026 time= 0.00195638 cumTime= 0.0359475 edges= 12556 schur= 0 lambda= 4531101065827.846680 levenbergIter= 1
iteration= 13 chi2= 7270674.808132 time= 0.00198433 cumTime= 0.0379319 edges= 12556 schur= 0 lambda= 3020734043885.230957 levenbergIter= 1
iteration= 14 chi2= 7270673.196159 time= 0.00292117 cumTime= 0.040853 edges= 12556 schur= 0 lambda= 16110581567387.898438 levenbergIter= 3
iteration= 15 chi2= 7270672.394194 time= 0.00250246 cumTime= 0.0433555 edges= 12556 schur= 0 lambda= 21480775423183.863281 levenbergIter= 2
iteration= 16 chi2= 7270672.240369 time= 0.00399782 cumTime= 0.0473533 edges= 12556 schur= 0 lambda= 7332104677780092.000000 levenbergIter= 5
iteration= 17 chi2= 7270672.165100 time= 0.00297799 cumTime= 0.0503313 edges= 12556 schur= 0 lambda= 19552279140746912.000000 levenbergIter= 3
iteration= 18 chi2= 7270671.880704 time= 0.00246083 cumTime= 0.0527921 edges= 12556 schur= 0 lambda= 13034852760497940.000000 levenbergIter= 2
iteration= 19 chi2= 7270671.843933 time= 0.00298128 cumTime= 0.0557734 edges= 12556 schur= 0 lambda= 34759607361327840.000000 levenbergIter= 3
iteration= 20 chi2= 7270671.843933 time= 0.00399836 cumTime= 0.0597718 edges= 12556 schur= 0 lambda= 1139002814015990661120.000000 levenbergIter= 5
direct method costs time: 0.0838985 seconds.
Tcw= 0.99766 0.0570194 -0.0377176 -0.040892
-0.056826 0.998365 0.0061796 -0.0340115
0.0380083 -0.00402179 0.999269 0.141452
0 0 0 1
*********** loop 4 ************
edges in graph: 12556
iteration= 0 chi2= 74528187.132917 time= 0.00219583 cumTime= 0.00219583 edges= 12556 schur= 0 lambda= 5131511.938933 levenbergIter= 1
iteration= 1 chi2= 57849063.123861 time= 0.00190137 cumTime= 0.0040972 edges= 12556 schur= 0 lambda= 1710503.979644 levenbergIter= 1
iteration= 2 chi2= 46261275.030205 time= 0.00197663 cumTime= 0.00607383 edges= 12556 schur= 0 lambda= 570167.993215 levenbergIter= 1
iteration= 3 chi2= 38125853.635180 time= 0.00189239 cumTime= 0.00796622 edges= 12556 schur= 0 lambda= 190055.997738 levenbergIter= 1
iteration= 4 chi2= 31251281.126722 time= 0.00201128 cumTime= 0.0099775 edges= 12556 schur= 0 lambda= 63351.999246 levenbergIter= 1
iteration= 5 chi2= 20637359.085714 time= 0.00213187 cumTime= 0.0121094 edges= 12556 schur= 0 lambda= 21117.333082 levenbergIter= 1
iteration= 6 chi2= 11082708.106920 time= 0.00198391 cumTime= 0.0140933 edges= 12556 schur= 0 lambda= 7039.111027 levenbergIter= 1
iteration= 7 chi2= 10014886.252565 time= 0.00191955 cumTime= 0.0160128 edges= 12556 schur= 0 lambda= 2346.370342 levenbergIter= 1
iteration= 8 chi2= 9933557.358926 time= 0.00198484 cumTime= 0.0179977 edges= 12556 schur= 0 lambda= 782.123447 levenbergIter= 1
iteration= 9 chi2= 9920569.797580 time= 0.00193966 cumTime= 0.0199373 edges= 12556 schur= 0 lambda= 260.707816 levenbergIter= 1
iteration= 10 chi2= 9919401.318880 time= 0.00197849 cumTime= 0.0219158 edges= 12556 schur= 0 lambda= 86.902605 levenbergIter= 1
iteration= 11 chi2= 9919256.492111 time= 0.00198626 cumTime= 0.0239021 edges= 12556 schur= 0 lambda= 56.462122 levenbergIter= 1
iteration= 12 chi2= 9919202.892643 time= 0.00604598 cumTime= 0.0299481 edges= 12556 schur= 0 lambda= 1293349153237.299316 levenbergIter= 9
iteration= 13 chi2= 9919187.569843 time= 0.00195102 cumTime= 0.0318991 edges= 12556 schur= 0 lambda= 431116384412.433105 levenbergIter= 1
iteration= 14 chi2= 9919179.614289 time= 0.00299901 cumTime= 0.0348981 edges= 12556 schur= 0 lambda= 1149643691766.488281 levenbergIter= 3
iteration= 15 chi2= 9919176.596775 time= 0.00295571 cumTime= 0.0378538 edges= 12556 schur= 0 lambda= 3065716511377.301758 levenbergIter= 3
iteration= 16 chi2= 9919176.215136 time= 0.00415 cumTime= 0.0420038 edges= 12556 schur= 0 lambda= 1046431235883452.250000 levenbergIter= 5
iteration= 17 chi2= 9919175.439131 time= 0.00188557 cumTime= 0.0438894 edges= 12556 schur= 0 lambda= 348810411961150.750000 levenbergIter= 1
iteration= 18 chi2= 9919175.053316 time= 0.00303776 cumTime= 0.0469271 edges= 12556 schur= 0 lambda= 930161098563068.625000 levenbergIter= 3
iteration= 19 chi2= 9919175.053316 time= 0.00403908 cumTime= 0.0509662 edges= 12556 schur= 0 lambda= 30479518877714632704.000000 levenbergIter= 5
direct method costs time: 0.067502 seconds.
Tcw= 0.995639 0.0747533 -0.055816 -0.0336959
-0.0740798 0.997153 0.0140426 -0.0449229
0.0567068 -0.00984653 0.998342 0.201114
0 0 0 1
*********** loop 5 ************
edges in graph: 12556
iteration= 0 chi2= 72822392.630478 time= 0.00215848 cumTime= 0.00215848 edges= 12556 schur= 0 lambda= 3545280.405102 levenbergIter= 1
iteration= 1 chi2= 61374186.635502 time= 0.00189387 cumTime= 0.00405235 edges= 12556 schur= 0 lambda= 1181760.135034 levenbergIter= 1
iteration= 2 chi2= 48972843.436029 time= 0.00194134 cumTime= 0.00599369 edges= 12556 schur= 0 lambda= 393920.045011 levenbergIter= 1
iteration= 3 chi2= 39959163.565811 time= 0.0018923 cumTime= 0.00788599 edges= 12556 schur= 0 lambda= 131306.681670 levenbergIter= 1
iteration= 4 chi2= 36851109.049488 time= 0.00197016 cumTime= 0.00985615 edges= 12556 schur= 0 lambda= 43768.893890 levenbergIter= 1
iteration= 5 chi2= 34956713.601657 time= 0.00196167 cumTime= 0.0118178 edges= 12556 schur= 0 lambda= 14589.631297 levenbergIter= 1
iteration= 6 chi2= 33337192.904026 time= 0.00188841 cumTime= 0.0137062 edges= 12556 schur= 0 lambda= 4863.210432 levenbergIter= 1
iteration= 7 chi2= 31314589.024519 time= 0.00211784 cumTime= 0.0158241 edges= 12556 schur= 0 lambda= 1621.070144 levenbergIter= 1
iteration= 8 chi2= 28311905.356978 time= 0.00207751 cumTime= 0.0179016 edges= 12556 schur= 0 lambda= 540.356715 levenbergIter= 1
iteration= 9 chi2= 22744726.531003 time= 0.00188491 cumTime= 0.0197865 edges= 12556 schur= 0 lambda= 180.118905 levenbergIter= 1
iteration= 10 chi2= 13319736.610417 time= 0.00196218 cumTime= 0.0217487 edges= 12556 schur= 0 lambda= 60.039635 levenbergIter= 1
iteration= 11 chi2= 13072528.635744 time= 0.00188421 cumTime= 0.0236329 edges= 12556 schur= 0 lambda= 29.211134 levenbergIter= 1
iteration= 12 chi2= 13048993.595588 time= 0.00196133 cumTime= 0.0255942 edges= 12556 schur= 0 lambda= 9.737045 levenbergIter= 1
iteration= 13 chi2= 13045628.530237 time= 0.00194759 cumTime= 0.0275418 edges= 12556 schur= 0 lambda= 3.245682 levenbergIter= 1
iteration= 14 chi2= 13044153.419754 time= 0.00188537 cumTime= 0.0294272 edges= 12556 schur= 0 lambda= 1.081894 levenbergIter= 1
iteration= 15 chi2= 13043655.891602 time= 0.00195245 cumTime= 0.0313796 edges= 12556 schur= 0 lambda= 0.360631 levenbergIter= 1
iteration= 16 chi2= 13043406.983842 time= 0.00207174 cumTime= 0.0334513 edges= 12556 schur= 0 lambda= 0.120210 levenbergIter= 1
iteration= 17 chi2= 13043287.480892 time= 0.00188976 cumTime= 0.0353411 edges= 12556 schur= 0 lambda= 0.040070 levenbergIter= 1
iteration= 18 chi2= 13043225.015030 time= 0.00200682 cumTime= 0.0373479 edges= 12556 schur= 0 lambda= 0.013357 levenbergIter= 1
iteration= 19 chi2= 13043197.342985 time= 0.00194669 cumTime= 0.0392946 edges= 12556 schur= 0 lambda= 0.004452 levenbergIter= 1
iteration= 20 chi2= 13043184.290833 time= 0.00190651 cumTime= 0.0412011 edges= 12556 schur= 0 lambda= 0.001484 levenbergIter= 1
iteration= 21 chi2= 13043172.729153 time= 0.00193715 cumTime= 0.0431383 edges= 12556 schur= 0 lambda= 0.000495 levenbergIter= 1
iteration= 22 chi2= 13043171.061467 time= 0.00192762 cumTime= 0.0450659 edges= 12556 schur= 0 lambda= 0.000165 levenbergIter= 1
iteration= 23 chi2= 13043170.864410 time= 0.00196113 cumTime= 0.047027 edges= 12556 schur= 0 lambda= 0.000055 levenbergIter= 1
iteration= 24 chi2= 13043170.864410 time= 0.00679344 cumTime= 0.0538205 edges= 12556 schur= 0 lambda= 1980355298773.438721 levenbergIter= 10
direct method costs time: 0.0728578 seconds.
Tcw= 0.993432 0.0908216 -0.0696016 -0.0234251
-0.0889695 0.995604 0.0292704 -0.0675555
0.071954 -0.0228857 0.997145 0.271233
0 0 0 1
*********** loop 6 ************
edges in graph: 12556
iteration= 0 chi2= 72615339.254368 time= 0.00413464 cumTime= 0.00413464 edges= 12556 schur= 0 lambda= 2577889.220350 levenbergIter= 1
iteration= 1 chi2= 55594053.158943 time= 0.00332422 cumTime= 0.00745886 edges= 12556 schur= 0 lambda= 859296.406783 levenbergIter= 1
iteration= 2 chi2= 42163490.895525 time= 0.00301068 cumTime= 0.0104695 edges= 12556 schur= 0 lambda= 286432.135594 levenbergIter= 1
iteration= 3 chi2= 37599642.045044 time= 0.00271699 cumTime= 0.0131865 edges= 12556 schur= 0 lambda= 95477.378531 levenbergIter= 1
iteration= 4 chi2= 36148957.061350 time= 0.00256971 cumTime= 0.0157562 edges= 12556 schur= 0 lambda= 31825.792844 levenbergIter= 1
iteration= 5 chi2= 34798934.282089 time= 0.00231479 cumTime= 0.018071 edges= 12556 schur= 0 lambda= 10608.597615 levenbergIter= 1
iteration= 6 chi2= 33275148.745603 time= 0.0022452 cumTime= 0.0203162 edges= 12556 schur= 0 lambda= 3536.199205 levenbergIter= 1
iteration= 7 chi2= 31158561.483538 time= 0.00209682 cumTime= 0.022413 edges= 12556 schur= 0 lambda= 1178.733068 levenbergIter= 1
iteration= 8 chi2= 27336828.704135 time= 0.0020116 cumTime= 0.0244246 edges= 12556 schur= 0 lambda= 392.911023 levenbergIter= 1
iteration= 9 chi2= 18521528.753670 time= 0.00198606 cumTime= 0.0264107 edges= 12556 schur= 0 lambda= 130.970341 levenbergIter= 1
iteration= 10 chi2= 14982202.879845 time= 0.00191272 cumTime= 0.0283234 edges= 12556 schur= 0 lambda= 43.656780 levenbergIter= 1
iteration= 11 chi2= 14892926.072438 time= 0.00193932 cumTime= 0.0302627 edges= 12556 schur= 0 lambda= 14.552260 levenbergIter= 1
iteration= 12 chi2= 14887641.887668 time= 0.0019925 cumTime= 0.0322552 edges= 12556 schur= 0 lambda= 4.850753 levenbergIter= 1
iteration= 13 chi2= 14886891.577526 time= 0.00188202 cumTime= 0.0341373 edges= 12556 schur= 0 lambda= 3.233836 levenbergIter= 1
iteration= 14 chi2= 14886648.587024 time= 0.00614012 cumTime= 0.0402774 edges= 12556 schur= 0 lambda= 74075829596.988525 levenbergIter= 9
iteration= 15 chi2= 14886594.621617 time= 0.00298954 cumTime= 0.0432669 edges= 12556 schur= 0 lambda= 197535545591.969391 levenbergIter= 3
iteration= 16 chi2= 14886584.739849 time= 0.00297924 cumTime= 0.0462462 edges= 12556 schur= 0 lambda= 636632444539.763306 levenbergIter= 3
iteration= 17 chi2= 14886578.872723 time= 0.00289478 cumTime= 0.0491409 edges= 12556 schur= 0 lambda= 1697686518772.702148 levenbergIter= 3
iteration= 18 chi2= 14886577.419458 time= 0.00238787 cumTime= 0.0515288 edges= 12556 schur= 0 lambda= 2263582025030.269531 levenbergIter= 2
iteration= 19 chi2= 14886576.937712 time= 0.00245274 cumTime= 0.0539816 edges= 12556 schur= 0 lambda= 3018109366707.025879 levenbergIter= 2
iteration= 20 chi2= 14886576.864661 time= 0.0044903 cumTime= 0.0584719 edges= 12556 schur= 0 lambda= 32965802576085272.000000 levenbergIter= 6
iteration= 21 chi2= 14886576.864661 time= 0.00359365 cumTime= 0.0620655 edges= 12556 schur= 0 lambda= 33756981837911318528.000000 levenbergIter= 4
direct method costs time: 0.086651 seconds.
Tcw= 0.991776 0.103867 -0.0747781 -0.0331323
-0.100849 0.993969 0.0430708 -0.0772277
0.0788007 -0.0351753 0.99627 0.346411
0 0 0 1
*********** loop 7 ************
edges in graph: 12556
iteration= 0 chi2= 76126715.275080 time= 0.00484098 cumTime= 0.00484098 edges= 12556 schur= 0 lambda= 3403852.459597 levenbergIter= 1
iteration= 1 chi2= 72769339.434144 time= 0.00371563 cumTime= 0.00855661 edges= 12556 schur= 0 lambda= 1134617.486532 levenbergIter= 1
iteration= 2 chi2= 66841216.326109 time= 0.00319426 cumTime= 0.0117509 edges= 12556 schur= 0 lambda= 378205.828844 levenbergIter= 1
iteration= 3 chi2= 58061634.010826 time= 0.00290521 cumTime= 0.0146561 edges= 12556 schur= 0 lambda= 126068.609615 levenbergIter= 1
iteration= 4 chi2= 48658865.716651 time= 0.00275282 cumTime= 0.0174089 edges= 12556 schur= 0 lambda= 42022.869872 levenbergIter= 1
iteration= 5 chi2= 39343059.583034 time= 0.0025716 cumTime= 0.0199805 edges= 12556 schur= 0 lambda= 14007.623291 levenbergIter= 1
iteration= 6 chi2= 37402395.425229 time= 0.00230087 cumTime= 0.0222814 edges= 12556 schur= 0 lambda= 4669.207764 levenbergIter= 1
iteration= 7 chi2= 36103537.153666 time= 0.00225346 cumTime= 0.0245348 edges= 12556 schur= 0 lambda= 1556.402588 levenbergIter= 1
iteration= 8 chi2= 34739663.388504 time= 0.0020494 cumTime= 0.0265842 edges= 12556 schur= 0 lambda= 518.800863 levenbergIter= 1
iteration= 9 chi2= 33166637.934856 time= 0.0020558 cumTime= 0.02864 edges= 12556 schur= 0 lambda= 172.933621 levenbergIter= 1
iteration= 10 chi2= 31476796.423510 time= 0.00192857 cumTime= 0.0305686 edges= 12556 schur= 0 lambda= 57.644540 levenbergIter= 1
iteration= 11 chi2= 29595556.083057 time= 0.00187781 cumTime= 0.0324464 edges= 12556 schur= 0 lambda= 19.214847 levenbergIter= 1
iteration= 12 chi2= 26536887.307927 time= 0.00194151 cumTime= 0.0343879 edges= 12556 schur= 0 lambda= 6.404949 levenbergIter= 1
iteration= 13 chi2= 20119967.950432 time= 0.00187897 cumTime= 0.0362669 edges= 12556 schur= 0 lambda= 2.134983 levenbergIter= 1
iteration= 14 chi2= 13999752.913560 time= 0.00187815 cumTime= 0.038145 edges= 12556 schur= 0 lambda= 0.711661 levenbergIter= 1
iteration= 15 chi2= 13896830.877163 time= 0.00196864 cumTime= 0.0401137 edges= 12556 schur= 0 lambda= 0.237220 levenbergIter= 1
iteration= 16 chi2= 13883130.474936 time= 0.00188911 cumTime= 0.0420028 edges= 12556 schur= 0 lambda= 0.079073 levenbergIter= 1
iteration= 17 chi2= 13881070.475414 time= 0.00194151 cumTime= 0.0439443 edges= 12556 schur= 0 lambda= 0.026358 levenbergIter= 1
iteration= 18 chi2= 13880532.047567 time= 0.00204951 cumTime= 0.0459938 edges= 12556 schur= 0 lambda= 0.008786 levenbergIter= 1
iteration= 19 chi2= 13880185.174383 time= 0.00187825 cumTime= 0.0478721 edges= 12556 schur= 0 lambda= 0.002929 levenbergIter= 1
iteration= 20 chi2= 13880043.513566 time= 0.00198442 cumTime= 0.0498565 edges= 12556 schur= 0 lambda= 0.000976 levenbergIter= 1
iteration= 21 chi2= 13879945.953482 time= 0.00192958 cumTime= 0.0517861 edges= 12556 schur= 0 lambda= 0.000325 levenbergIter= 1
iteration= 22 chi2= 13879890.851421 time= 0.001879 cumTime= 0.0536651 edges= 12556 schur= 0 lambda= 0.000108 levenbergIter= 1
iteration= 23 chi2= 13879859.084591 time= 0.00193961 cumTime= 0.0556047 edges= 12556 schur= 0 lambda= 0.000036 levenbergIter= 1
iteration= 24 chi2= 13879845.444717 time= 0.00190357 cumTime= 0.0575082 edges= 12556 schur= 0 lambda= 0.000012 levenbergIter= 1
iteration= 25 chi2= 13879834.866027 time= 0.00197325 cumTime= 0.0594815 edges= 12556 schur= 0 lambda= 0.000004 levenbergIter= 1
iteration= 26 chi2= 13879826.914010 time= 0.00197705 cumTime= 0.0614585 edges= 12556 schur= 0 lambda= 0.000001 levenbergIter= 1
iteration= 27 chi2= 13879823.187287 time= 0.00189244 cumTime= 0.063351 edges= 12556 schur= 0 lambda= 0.000000 levenbergIter= 1
iteration= 28 chi2= 13879820.362015 time= 0.00201701 cumTime= 0.065368 edges= 12556 schur= 0 lambda= 0.000000 levenbergIter= 1
iteration= 29 chi2= 13879818.220320 time= 0.00202852 cumTime= 0.0673965 edges= 12556 schur= 0 lambda= 0.000000 levenbergIter= 1
direct method costs time: 0.0999934 seconds.
Tcw= 0.990165 0.112939 -0.0825744 -0.0265597
-0.108131 0.99229 0.0605637 -0.110576
0.0887777 -0.0510393 0.994743 0.437234
0 0 0 1
*********** loop 8 ************
edges in graph: 12556
iteration= 0 chi2= 84175871.369677 time= 0.0040917 cumTime= 0.0040917 edges= 12556 schur= 0 lambda= 2380068.700399 levenbergIter= 1
iteration= 1 chi2= 61078995.869678 time= 0.00331044 cumTime= 0.00740214 edges= 12556 schur= 0 lambda= 793356.233466 levenbergIter= 1
iteration= 2 chi2= 44678686.954011 time= 0.00289414 cumTime= 0.0102963 edges= 12556 schur= 0 lambda= 264452.077822 levenbergIter= 1
iteration= 3 chi2= 37291882.913976 time= 0.00269438 cumTime= 0.0129907 edges= 12556 schur= 0 lambda= 88150.692607 levenbergIter= 1
iteration= 4 chi2= 35821302.991413 time= 0.0025769 cumTime= 0.0155675 edges= 12556 schur= 0 lambda= 29383.564202 levenbergIter= 1
iteration= 5 chi2= 34387389.611984 time= 0.00230761 cumTime= 0.0178752 edges= 12556 schur= 0 lambda= 9794.521401 levenbergIter= 1
iteration= 6 chi2= 33699142.598652 time= 0.00223297 cumTime= 0.0201081 edges= 12556 schur= 0 lambda= 3264.840467 levenbergIter= 1
iteration= 7 chi2= 33376151.465511 time= 0.00209239 cumTime= 0.0222005 edges= 12556 schur= 0 lambda= 1088.280156 levenbergIter= 1
iteration= 8 chi2= 33062210.636318 time= 0.00200785 cumTime= 0.0242084 edges= 12556 schur= 0 lambda= 362.760052 levenbergIter= 1
iteration= 9 chi2= 32653345.338549 time= 0.00191657 cumTime= 0.0261249 edges= 12556 schur= 0 lambda= 120.920017 levenbergIter= 1
iteration= 10 chi2= 31975365.668054 time= 0.00194104 cumTime= 0.028066 edges= 12556 schur= 0 lambda= 40.306672 levenbergIter= 1
iteration= 11 chi2= 30783352.054369 time= 0.00199963 cumTime= 0.0300656 edges= 12556 schur= 0 lambda= 13.435557 levenbergIter= 1
iteration= 12 chi2= 28246380.871980 time= 0.00193039 cumTime= 0.031996 edges= 12556 schur= 0 lambda= 4.478519 levenbergIter= 1
iteration= 13 chi2= 22550913.596786 time= 0.00191856 cumTime= 0.0339146 edges= 12556 schur= 0 lambda= 1.492840 levenbergIter= 1
iteration= 14 chi2= 17277300.681064 time= 0.00192574 cumTime= 0.0358403 edges= 12556 schur= 0 lambda= 0.497613 levenbergIter= 1
iteration= 15 chi2= 16389927.449531 time= 0.00195607 cumTime= 0.0377964 edges= 12556 schur= 0 lambda= 0.165871 levenbergIter= 1
iteration= 16 chi2= 16289347.994950 time= 0.00191979 cumTime= 0.0397162 edges= 12556 schur= 0 lambda= 0.055290 levenbergIter= 1
iteration= 17 chi2= 16278979.287526 time= 0.0019173 cumTime= 0.0416335 edges= 12556 schur= 0 lambda= 0.018430 levenbergIter= 1
iteration= 18 chi2= 16278266.802790 time= 0.00664827 cumTime= 0.0482817 edges= 12556 schur= 0 lambda= 216150733194.204865 levenbergIter= 10
direct method costs time: 0.0711682 seconds.
Tcw= 0.988641 0.118336 -0.0926614 0.0343184
-0.111659 0.990971 0.0742193 -0.1247
0.100608 -0.0630298 0.992928 0.509306
0 0 0 1
*********** loop 9 ************
将点的大小改小后：
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 2, cv::Scalar ( b,g,r ), 1 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 2, cv::Scalar ( b,g,r ), 1 ); 
direct_sparse.cpp
#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.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;
/********************************************
* 本节演示了RGBD上的稀疏直接法
********************************************/
// 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
struct Measurement
{
Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
Eigen::Vector3d pos_world;
float grayscale;
};
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
float zz = float ( d ) /scale;
float xx = zz* ( x-cx ) /fx;
float yy = zz* ( y-cy ) /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 )
{
float u = fx*x/z+cx;
float v = fy*y/z+cy;
return Eigen::Vector2d ( u,v );
}
// 直接法估计位姿
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
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 )
{
if ( argc != 2 )
{
cout<<"usage: useLK path_to_dataset"<<endl;
return 1;
}
srand ( ( unsigned int ) time ( 0 ) );
string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";
ifstream fin ( associate_file );
string rgb_file, depth_file, time_rgb, time_depth;
cv::Mat color, depth, gray;
vector<Measurement> measurements;
// 相机内参
float cx = 325.5;
float cy = 253.5;
float fx = 518.0;
float fy = 519.0;
float depth_scale = 1000.0;
Eigen::Matrix3f K;
K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
cv::Mat prev_color;
// 我们以第一个图像为参考，对后续图像和参考图像做直接法
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 )
{
// 对第一帧提取FAST特征点
vector<cv::KeyPoint> keypoints;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect ( color, keypoints );
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 )
continue;
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( 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> ( cvRound ( kp.pt.y ) ) [ cvRound ( 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 );
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 ) ) );
for ( Measurement m:measurements )
{
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::Vector3d p2 = Tcw*m.pos_world;
Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
continue;
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 );
}
return 0;
}
bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
// 初始化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 );
//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
std::unique_ptr<DirectBlock::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>());
std::unique_ptr<DirectBlock> solver_ptr ( new DirectBlock ( std::move(linearSolver)));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
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 )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
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 );
}
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
optimizer.initializeOptimization();
optimizer.optimize ( 30 );
Tcw = pose->estimate();
}
CMakeLists.txt
和上面一样
执行结果：
./direct_sparse ../../data
*********** loop 0 ************
*********** loop 1 ************
edges in graph: 1402
iteration= 0 chi2= 6293572.221433 time= 0.000296299 cumTime= 0.000296299 edges= 1402 schur= 0 lambda= 965806.498992 levenbergIter= 1
iteration= 1 chi2= 5888155.828834 time= 0.000251655 cumTime= 0.000547954 edges= 1402 schur= 0 lambda= 321935.499664 levenbergIter= 1
iteration= 2 chi2= 5548017.147678 time= 0.000250156 cumTime= 0.00079811 edges= 1402 schur= 0 lambda= 107311.833221 levenbergIter= 1
iteration= 3 chi2= 5290937.995946 time= 0.000249944 cumTime= 0.00104805 edges= 1402 schur= 0 lambda= 35770.611074 levenbergIter= 1
iteration= 4 chi2= 5207371.610703 time= 0.000250005 cumTime= 0.00129806 edges= 1402 schur= 0 lambda= 11923.537025 levenbergIter= 1
iteration= 5 chi2= 5156306.765017 time= 0.000253176 cumTime= 0.00155124 edges= 1402 schur= 0 lambda= 3974.512342 levenbergIter= 1
iteration= 6 chi2= 5133807.422806 time= 0.000250664 cumTime= 0.0018019 edges= 1402 schur= 0 lambda= 1324.837447 levenbergIter= 1
iteration= 7 chi2= 5124650.368695 time= 0.000250457 cumTime= 0.00205236 edges= 1402 schur= 0 lambda= 441.612482 levenbergIter= 1
iteration= 8 chi2= 5112774.456392 time= 0.00027613 cumTime= 0.00232849 edges= 1402 schur= 0 lambda= 147.204161 levenbergIter= 1
iteration= 9 chi2= 5103863.403139 time= 0.000250364 cumTime= 0.00257885 edges= 1402 schur= 0 lambda= 49.068054 levenbergIter= 1
iteration= 10 chi2= 5085463.721599 time= 0.000250061 cumTime= 0.00282891 edges= 1402 schur= 0 lambda= 16.356018 levenbergIter= 1
iteration= 11 chi2= 5052922.896857 time= 0.000250117 cumTime= 0.00307903 edges= 1402 schur= 0 lambda= 5.452006 levenbergIter= 1
iteration= 12 chi2= 4972629.841316 time= 0.000249897 cumTime= 0.00332893 edges= 1402 schur= 0 lambda= 1.817335 levenbergIter= 1
iteration= 13 chi2= 4900169.515522 time= 0.000250179 cumTime= 0.0035791 edges= 1402 schur= 0 lambda= 0.605778 levenbergIter= 1
iteration= 14 chi2= 4888003.377098 time= 0.000249578 cumTime= 0.00382868 edges= 1402 schur= 0 lambda= 0.201926 levenbergIter= 1
iteration= 15 chi2= 4887020.906430 time= 0.000228279 cumTime= 0.00405696 edges= 1402 schur= 0 lambda= 0.134617 levenbergIter= 1
iteration= 16 chi2= 4886717.215284 time= 0.000656648 cumTime= 0.00471361 edges= 1402 schur= 0 lambda= 24090727.650511 levenbergIter= 8
iteration= 17 chi2= 4885996.489698 time= 0.000228124 cumTime= 0.00494173 edges= 1402 schur= 0 lambda= 8030242.550170 levenbergIter= 1
iteration= 18 chi2= 4885632.694219 time= 0.000228245 cumTime= 0.00516998 edges= 1402 schur= 0 lambda= 5353495.033447 levenbergIter= 1
iteration= 19 chi2= 4885625.691907 time= 0.000457031 cumTime= 0.00562701 edges= 1402 schur= 0 lambda= 228415788.093730 levenbergIter= 4
iteration= 20 chi2= 4885387.309209 time= 0.000228231 cumTime= 0.00585524 edges= 1402 schur= 0 lambda= 76138596.031243 levenbergIter= 1
iteration= 21 chi2= 4885212.916240 time= 0.000228318 cumTime= 0.00608356 edges= 1402 schur= 0 lambda= 25379532.010414 levenbergIter= 1
iteration= 22 chi2= 4885046.727489 time= 0.000228051 cumTime= 0.00631161 edges= 1402 schur= 0 lambda= 8459844.003471 levenbergIter= 1
iteration= 23 chi2= 4884831.772796 time= 0.000228395 cumTime= 0.00654 edges= 1402 schur= 0 lambda= 2819948.001157 levenbergIter= 1
iteration= 24 chi2= 4884332.947280 time= 0.00022803 cumTime= 0.00676803 edges= 1402 schur= 0 lambda= 939982.667052 levenbergIter= 1
iteration= 25 chi2= 4883825.913749 time= 0.000227988 cumTime= 0.00699602 edges= 1402 schur= 0 lambda= 313327.555684 levenbergIter= 1
iteration= 26 chi2= 4883383.826737 time= 0.000228086 cumTime= 0.00722411 edges= 1402 schur= 0 lambda= 104442.518561 levenbergIter= 1
iteration= 27 chi2= 4883030.529985 time= 0.000228103 cumTime= 0.00745221 edges= 1402 schur= 0 lambda= 34814.172854 levenbergIter= 1
iteration= 28 chi2= 4882854.735927 time= 0.000239215 cumTime= 0.00769143 edges= 1402 schur= 0 lambda= 11604.724285 levenbergIter= 1
iteration= 29 chi2= 4882587.766097 time= 0.000265416 cumTime= 0.00795684 edges= 1402 schur= 0 lambda= 3868.241428 levenbergIter= 1
direct method costs time: 0.0110911 seconds.
Tcw= 0.99906 0.0147107 0.0407838 -0.272174
-0.0161384 0.99926 0.034902 -0.207344
-0.0402403 -0.0355274 0.998558 -0.089623
0 0 0 1
*********** loop 2 ************
edges in graph: 1402
iteration= 0 chi2= 7373902.978023 time= 0.000319957 cumTime= 0.000319957 edges= 1402 schur= 0 lambda= 659462.718469 levenbergIter= 1
iteration= 1 chi2= 7318224.416046 time= 0.000316246 cumTime= 0.000636203 edges= 1402 schur= 0 lambda= 219820.906156 levenbergIter= 1
iteration= 2 chi2= 7273774.690150 time= 0.00028491 cumTime= 0.000921113 edges= 1402 schur= 0 lambda= 73273.635385 levenbergIter= 1
iteration= 3 chi2= 7207104.517968 time= 0.000346131 cumTime= 0.00126724 edges= 1402 schur= 0 lambda= 24424.545128 levenbergIter= 1
iteration= 4 chi2= 7086855.080221 time= 0.000396316 cumTime= 0.00166356 edges= 1402 schur= 0 lambda= 8141.515043 levenbergIter= 1
iteration= 5 chi2= 6851372.489263 time= 0.000222192 cumTime= 0.00188575 edges= 1402 schur= 0 lambda= 2713.838348 levenbergIter= 1
iteration= 6 chi2= 6363864.651418 time= 0.000222395 cumTime= 0.00210815 edges= 1402 schur= 0 lambda= 904.612783 levenbergIter= 1
iteration= 7 chi2= 5992247.916295 time= 0.000241029 cumTime= 0.00234918 edges= 1402 schur= 0 lambda= 301.537594 levenbergIter= 1
iteration= 8 chi2= 5648821.981689 time= 0.000222741 cumTime= 0.00257192 edges= 1402 schur= 0 lambda= 100.512531 levenbergIter= 1
iteration= 9 chi2= 5524363.392931 time= 0.000222956 cumTime= 0.00279487 edges= 1402 schur= 0 lambda= 33.504177 levenbergIter= 1
iteration= 10 chi2= 5428608.363880 time= 0.000245203 cumTime= 0.00304008 edges= 1402 schur= 0 lambda= 11.168059 levenbergIter= 1
iteration= 11 chi2= 5275088.252740 time= 0.000286814 cumTime= 0.00332689 edges= 1402 schur= 0 lambda= 3.722686 levenbergIter= 1
iteration= 12 chi2= 5102935.213991 time= 0.000283679 cumTime= 0.00361057 edges= 1402 schur= 0 lambda= 1.240895 levenbergIter= 1
iteration= 13 chi2= 5013617.434958 time= 0.000283122 cumTime= 0.00389369 edges= 1402 schur= 0 lambda= 0.413632 levenbergIter= 1
iteration= 14 chi2= 4989686.431377 time= 0.000257568 cumTime= 0.00415126 edges= 1402 schur= 0 lambda= 0.137877 levenbergIter= 1
iteration= 15 chi2= 4974934.845258 time= 0.000244375 cumTime= 0.00439563 edges= 1402 schur= 0 lambda= 0.045959 levenbergIter= 1
iteration= 16 chi2= 4969876.746407 time= 0.000351987 cumTime= 0.00474762 edges= 1402 schur= 0 lambda= 0.015320 levenbergIter= 1
iteration= 17 chi2= 4966864.535229 time= 0.000243865 cumTime= 0.00499149 edges= 1402 schur= 0 lambda= 0.005107 levenbergIter= 1
iteration= 18 chi2= 4965509.741089 time= 0.000230896 cumTime= 0.00522238 edges= 1402 schur= 0 lambda= 0.003404 levenbergIter= 1
iteration= 19 chi2= 4964542.688070 time= 0.000230675 cumTime= 0.00545306 edges= 1402 schur= 0 lambda= 0.001135 levenbergIter= 1
iteration= 20 chi2= 4963641.818600 time= 0.000252545 cumTime= 0.0057056 edges= 1402 schur= 0 lambda= 0.000378 levenbergIter= 1
iteration= 21 chi2= 4963541.722042 time= 0.000230281 cumTime= 0.00593588 edges= 1402 schur= 0 lambda= 0.000252 levenbergIter= 1
iteration= 22 chi2= 4963541.722042 time= 0.000847664 cumTime= 0.00678355 edges= 1402 schur= 0 lambda= 9085600825972.755859 levenbergIter= 10
direct method costs time: 0.00999512 seconds.
Tcw= 0.99878 0.0347253 0.0350956 -0.308517
-0.0360817 0.998596 0.0387864 -0.228252
-0.0336994 -0.0400054 0.998631 -0.05351
0 0 0 1
*********** loop 3 ************
edges in graph: 1402
iteration= 0 chi2= 7652391.296677 time= 0.00075729 cumTime= 0.00075729 edges= 1402 schur= 0 lambda= 488770.389310 levenbergIter= 1
iteration= 1 chi2= 7263810.066649 time= 0.0006484 cumTime= 0.00140569 edges= 1402 schur= 0 lambda= 162923.463103 levenbergIter= 1
iteration= 2 chi2= 7113930.131795 time= 0.000645919 cumTime= 0.00205161 edges= 1402 schur= 0 lambda= 54307.821034 levenbergIter= 1
iteration= 3 chi2= 6985494.549018 time= 0.000645862 cumTime= 0.00269747 edges= 1402 schur= 0 lambda= 18102.607011 levenbergIter= 1
iteration= 4 chi2= 6821206.708909 time= 0.000645833 cumTime= 0.0033433 edges= 1402 schur= 0 lambda= 6034.202337 levenbergIter= 1
iteration= 5 chi2= 6639561.237546 time= 0.000568494 cumTime= 0.0039118 edges= 1402 schur= 0 lambda= 2011.400779 levenbergIter= 1
iteration= 6 chi2= 6413013.798621 time= 0.000520107 cumTime= 0.0044319 edges= 1402 schur= 0 lambda= 670.466926 levenbergIter= 1
iteration= 7 chi2= 6226118.353718 time= 0.000519886 cumTime= 0.00495179 edges= 1402 schur= 0 lambda= 223.488975 levenbergIter= 1
iteration= 8 chi2= 6061742.336272 time= 0.000519493 cumTime= 0.00547128 edges= 1402 schur= 0 lambda= 74.496325 levenbergIter= 1
iteration= 9 chi2= 5844667.726463 time= 0.000519177 cumTime= 0.00599046 edges= 1402 schur= 0 lambda= 24.832108 levenbergIter= 1
iteration= 10 chi2= 5637484.497617 time= 0.000519263 cumTime= 0.00650972 edges= 1402 schur= 0 lambda= 8.277369 levenbergIter= 1
iteration= 11 chi2= 5425243.040401 time= 0.000522855 cumTime= 0.00703258 edges= 1402 schur= 0 lambda= 2.759123 levenbergIter= 1
iteration= 12 chi2= 5264243.471467 time= 0.000503762 cumTime= 0.00753634 edges= 1402 schur= 0 lambda= 0.919708 levenbergIter= 1
iteration= 13 chi2= 5137385.989498 time= 0.000435001 cumTime= 0.00797134 edges= 1402 schur= 0 lambda= 0.306569 levenbergIter= 1
iteration= 14 chi2= 5051110.224167 time= 0.000435244 cumTime= 0.00840659 edges= 1402 schur= 0 lambda= 0.102190 levenbergIter= 1
iteration= 15 chi2= 4942318.409186 time= 0.000435218 cumTime= 0.0088418 edges= 1402 schur= 0 lambda= 0.034063 levenbergIter= 1
iteration= 16 chi2= 4837907.001983 time= 0.000435068 cumTime= 0.00927687 edges= 1402 schur= 0 lambda= 0.011354 levenbergIter= 1
iteration= 17 chi2= 4673004.142642 time= 0.000434778 cumTime= 0.00971165 edges= 1402 schur= 0 lambda= 0.003785 levenbergIter= 1
iteration= 18 chi2= 4449426.526615 time= 0.000438196 cumTime= 0.0101498 edges= 1402 schur= 0 lambda= 0.001262 levenbergIter= 1
iteration= 19 chi2= 3973385.310078 time= 0.000434567 cumTime= 0.0105844 edges= 1402 schur= 0 lambda= 0.000421 levenbergIter= 1
iteration= 20 chi2= 2361801.867656 time= 0.000434547 cumTime= 0.011019 edges= 1402 schur= 0 lambda= 0.000140 levenbergIter= 1
iteration= 21 chi2= 1407201.072916 time= 0.000410939 cumTime= 0.0114299 edges= 1402 schur= 0 lambda= 0.000047 levenbergIter= 1
iteration= 22 chi2= 1225311.653677 time= 0.000373351 cumTime= 0.0118032 edges= 1402 schur= 0 lambda= 0.000031 levenbergIter= 1
iteration= 23 chi2= 1224758.010208 time= 0.00117959 cumTime= 0.0129828 edges= 1402 schur= 0 lambda= 1427104.791486 levenbergIter= 9
iteration= 24 chi2= 1222153.585816 time= 0.000374429 cumTime= 0.0133573 edges= 1402 schur= 0 lambda= 951403.194324 levenbergIter= 1
iteration= 25 chi2= 1220151.752588 time= 0.000575745 cumTime= 0.013933 edges= 1402 schur= 0 lambda= 5074150.369727 levenbergIter= 3
iteration= 26 chi2= 1218274.743458 time= 0.0003736 cumTime= 0.0143066 edges= 1402 schur= 0 lambda= 3382766.913152 levenbergIter= 1
iteration= 27 chi2= 1206895.182885 time= 0.000575998 cumTime= 0.0148826 edges= 1402 schur= 0 lambda= 18041423.536808 levenbergIter= 3
iteration= 28 chi2= 1205341.650285 time= 0.000475787 cumTime= 0.0153584 edges= 1402 schur= 0 lambda= 24055231.382411 levenbergIter= 2
iteration= 29 chi2= 1196693.739565 time= 0.000328038 cumTime= 0.0156864 edges= 1402 schur= 0 lambda= 16036820.921607 levenbergIter= 1
direct method costs time: 0.0223067 seconds.
Tcw= 0.997652 0.0587578 -0.0351798 -0.054183
-0.0584732 0.998248 0.00906807 -0.051229
0.035651 -0.0069897 0.99934 0.141057
0 0 0 1
*********** loop 4 ************
edges in graph: 1402
iteration= 0 chi2= 7491847.477723 time= 0.000748474 cumTime= 0.000748474 edges= 1402 schur= 0 lambda= 397252.334581 levenbergIter= 1
iteration= 1 chi2= 6557855.290093 time= 0.000653204 cumTime= 0.00140168 edges= 1402 schur= 0 lambda= 132417.444860 levenbergIter= 1
iteration= 2 chi2= 5673808.446398 time= 0.000643492 cumTime= 0.00204517 edges= 1402 schur= 0 lambda= 44139.148287 levenbergIter= 1
iteration= 3 chi2= 4565012.469547 time= 0.000643989 cumTime= 0.00268916 edges= 1402 schur= 0 lambda= 14713.049429 levenbergIter= 1
iteration= 4 chi2= 2829297.412509 time= 0.000633116 cumTime= 0.00332227 edges= 1402 schur= 0 lambda= 4904.349810 levenbergIter= 1
iteration= 5 chi2= 1913504.899716 time= 0.000517169 cumTime= 0.00383944 edges= 1402 schur= 0 lambda= 1634.783270 levenbergIter= 1
iteration= 6 chi2= 1618694.067285 time= 0.000521021 cumTime= 0.00436046 edges= 1402 schur= 0 lambda= 728.654097 levenbergIter= 1
iteration= 7 chi2= 1607378.256511 time= 0.000517005 cumTime= 0.00487747 edges= 1402 schur= 0 lambda= 485.769398 levenbergIter= 1
iteration= 8 chi2= 1562462.492374 time= 0.00149791 cumTime= 0.00637538 edges= 1402 schur= 0 lambda= 86931819878.871063 levenbergIter= 8
iteration= 9 chi2= 1561212.496415 time= 0.000516393 cumTime= 0.00689177 edges= 1402 schur= 0 lambda= 57954546585.914040 levenbergIter= 1
iteration= 10 chi2= 1560587.249912 time= 0.000498109 cumTime= 0.00738988 edges= 1402 schur= 0 lambda= 30176991782.787167 levenbergIter= 1
iteration= 11 chi2= 1560091.227612 time= 0.000433055 cumTime= 0.00782294 edges= 1402 schur= 0 lambda= 10058997260.929054 levenbergIter= 1
iteration= 12 chi2= 1559641.017176 time= 0.000432718 cumTime= 0.00825565 edges= 1402 schur= 0 lambda= 3352999086.976351 levenbergIter= 1
iteration= 13 chi2= 1559400.179395 time= 0.000432605 cumTime= 0.00868826 edges= 1402 schur= 0 lambda= 1117666362.325450 levenbergIter= 1
iteration= 14 chi2= 1559048.255427 time= 0.000432066 cumTime= 0.00912032 edges= 1402 schur= 0 lambda= 372555454.108483 levenbergIter= 1
iteration= 15 chi2= 1558342.034485 time= 0.000432605 cumTime= 0.00955293 edges= 1402 schur= 0 lambda= 124185151.369494 levenbergIter= 1
iteration= 16 chi2= 1557243.868854 time= 0.000432628 cumTime= 0.00998556 edges= 1402 schur= 0 lambda= 41395050.456498 levenbergIter= 1
iteration= 17 chi2= 1556380.140212 time= 0.000552212 cumTime= 0.0105378 edges= 1402 schur= 0 lambda= 27596700.304332 levenbergIter= 2
iteration= 18 chi2= 1556117.304496 time= 0.000666143 cumTime= 0.0112039 edges= 1402 schur= 0 lambda= 73591200.811552 levenbergIter= 3
iteration= 19 chi2= 1556117.073821 time= 0.000976006 cumTime= 0.0121799 edges= 1402 schur= 0 lambda= 51443977988116.140625 levenbergIter= 7
iteration= 20 chi2= 1556116.920788 time= 0.000371856 cumTime= 0.0125518 edges= 1402 schur= 0 lambda= 17147992662705.378906 levenbergIter= 1
iteration= 21 chi2= 1556116.920788 time= 0.000873694 cumTime= 0.0134255 edges= 1402 schur= 0 lambda= 35961947108577910784.000000 levenbergIter= 6
direct method costs time: 0.0188089 seconds.
Tcw= 0.995597 0.0785082 -0.0512126 -0.0576872
-0.0777067 0.996823 0.0174622 -0.0664859
0.0524208 -0.0134057 0.998535 0.202252
0 0 0 1
*********** loop 5 ************
edges in graph: 1402
iteration= 0 chi2= 6811632.327202 time= 0.000752507 cumTime= 0.000752507 edges= 1402 schur= 0 lambda= 269680.591723 levenbergIter= 1
iteration= 1 chi2= 6580154.523110 time= 0.000651252 cumTime= 0.00140376 edges= 1402 schur= 0 lambda= 89893.530574 levenbergIter= 1
iteration= 2 chi2= 6489551.409912 time= 0.000641536 cumTime= 0.0020453 edges= 1402 schur= 0 lambda= 29964.510191 levenbergIter= 1
iteration= 3 chi2= 6188916.273438 time= 0.000641524 cumTime= 0.00268682 edges= 1402 schur= 0 lambda= 9988.170064 levenbergIter= 1
iteration= 4 chi2= 5763280.416543 time= 0.000607579 cumTime= 0.0032944 edges= 1402 schur= 0 lambda= 3329.390021 levenbergIter= 1
iteration= 5 chi2= 5563292.424881 time= 0.000516869 cumTime= 0.00381127 edges= 1402 schur= 0 lambda= 1109.796674 levenbergIter= 1
iteration= 6 chi2= 5467085.205965 time= 0.000521933 cumTime= 0.0043332 edges= 1402 schur= 0 lambda= 369.932225 levenbergIter= 1
iteration= 7 chi2= 5045410.765883 time= 0.000516561 cumTime= 0.00484976 edges= 1402 schur= 0 lambda= 123.310742 levenbergIter= 1
iteration= 8 chi2= 4890895.420715 time= 0.000517288 cumTime= 0.00536705 edges= 1402 schur= 0 lambda= 47.033601 levenbergIter= 1
iteration= 9 chi2= 4880331.546214 time= 0.000515669 cumTime= 0.00588272 edges= 1402 schur= 0 lambda= 31.355734 levenbergIter= 1
iteration= 10 chi2= 4880145.770882 time= 0.0012293 cumTime= 0.00711202 edges= 1402 schur= 0 lambda= 684976.463660 levenbergIter= 6
iteration= 11 chi2= 4874904.462550 time= 0.000436081 cumTime= 0.0075481 edges= 1402 schur= 0 lambda= 456650.975774 levenbergIter= 1
iteration= 12 chi2= 4874537.812220 time= 0.000900784 cumTime= 0.00844889 edges= 1402 schur= 0 lambda= 311740399.461474 levenbergIter= 5
iteration= 13 chi2= 4872628.627136 time= 0.000432374 cumTime= 0.00888126 edges= 1402 schur= 0 lambda= 103913466.487158 levenbergIter= 1
iteration= 14 chi2= 4872628.097592 time= 0.00113465 cumTime= 0.0100159 edges= 1402 schur= 0 lambda= 72640778023492.000000 levenbergIter= 7
iteration= 15 chi2= 4872627.879060 time= 0.000431932 cumTime= 0.0104478 edges= 1402 schur= 0 lambda= 48427185348994.664062 levenbergIter= 1
iteration= 16 chi2= 4872627.855692 time= 0.000795701 cumTime= 0.0112435 edges= 1402 schur= 0 lambda= 1033113287445219.500000 levenbergIter= 4
iteration= 17 chi2= 4872627.765096 time= 0.000571611 cumTime= 0.0118152 edges= 1402 schur= 0 lambda= 2754968766520585.000000 levenbergIter= 3
iteration= 18 chi2= 4872627.565836 time= 0.000471919 cumTime= 0.0122871 edges= 1402 schur= 0 lambda= 1836645844347056.500000 levenbergIter= 2
iteration= 19 chi2= 4872627.565836 time= 0.000772497 cumTime= 0.0130596 edges= 1402 schur= 0 lambda= 60183211027564347392.000000 levenbergIter= 5
direct method costs time: 0.0181874 seconds.
Tcw= 0.995694 0.0868309 -0.0324615 -0.208598
-0.0869428 0.996211 -0.00204789 0.0937473
0.0321607 0.00486137 0.999471 0.246901
0 0 0 1
*********** loop 6 ************
edges in graph: 1402
iteration= 0 chi2= 7928699.317460 time= 0.000912469 cumTime= 0.000912469 edges= 1402 schur= 0 lambda= 199616.574791 levenbergIter= 1
iteration= 1 chi2= 7755798.242776 time= 0.000646986 cumTime= 0.00155945 edges= 1402 schur= 0 lambda= 66538.858264 levenbergIter= 1
iteration= 2 chi2= 7543664.737807 time= 0.000642943 cumTime= 0.0022024 edges= 1402 schur= 0 lambda= 22179.619421 levenbergIter= 1
iteration= 3 chi2= 7485356.894104 time= 0.000642122 cumTime= 0.00284452 edges= 1402 schur= 0 lambda= 7393.206474 levenbergIter= 1
iteration= 4 chi2= 7432757.242352 time= 0.000641602 cumTime= 0.00348612 edges= 1402 schur= 0 lambda= 2464.402158 levenbergIter= 1
iteration= 5 chi2= 7355677.978479 time= 0.000642367 cumTime= 0.00412849 edges= 1402 schur= 0 lambda= 821.467386 levenbergIter= 1
iteration= 6 chi2= 7224185.509870 time= 0.000540844 cumTime= 0.00466933 edges= 1402 schur= 0 lambda= 273.822462 levenbergIter= 1
iteration= 7 chi2= 7122505.620093 time= 0.000517482 cumTime= 0.00518681 edges= 1402 schur= 0 lambda= 91.274154 levenbergIter= 1
iteration= 8 chi2= 7066632.695692 time= 0.000516856 cumTime= 0.00570367 edges= 1402 schur= 0 lambda= 30.424718 levenbergIter= 1
iteration= 9 chi2= 7061683.354334 time= 0.000520503 cumTime= 0.00622417 edges= 1402 schur= 0 lambda= 20.283145 levenbergIter= 1
iteration= 10 chi2= 7061552.728401 time= 0.00163387 cumTime= 0.00785804 edges= 1402 schur= 0 lambda= 464615711273.290833 levenbergIter= 9
iteration= 11 chi2= 7061252.173599 time= 0.000520618 cumTime= 0.00837866 edges= 1402 schur= 0 lambda= 240379334294.572052 levenbergIter= 1
iteration= 12 chi2= 7061239.156459 time= 0.000434057 cumTime= 0.00881272 edges= 1402 schur= 0 lambda= 160252889529.714691 levenbergIter= 1
iteration= 13 chi2= 7061180.403825 time= 0.000672917 cumTime= 0.00948563 edges= 1402 schur= 0 lambda= 854682077491.811646 levenbergIter= 3
iteration= 14 chi2= 7061142.368735 time= 0.000549184 cumTime= 0.0100348 edges= 1402 schur= 0 lambda= 1139576103322.415527 levenbergIter= 2
iteration= 15 chi2= 7061138.455047 time= 0.000666032 cumTime= 0.0107008 edges= 1402 schur= 0 lambda= 6077739217719.548828 levenbergIter= 3
iteration= 16 chi2= 7061130.287448 time= 0.000433419 cumTime= 0.0111343 edges= 1402 schur= 0 lambda= 4051826145146.365723 levenbergIter= 1
iteration= 17 chi2= 7061122.977106 time= 0.000549705 cumTime= 0.011684 edges= 1402 schur= 0 lambda= 5402434860195.154297 levenbergIter= 2
iteration= 18 chi2= 7061122.520741 time= 0.000980642 cumTime= 0.0126646 edges= 1402 schur= 0 lambda= 59008995166291600.000000 levenbergIter= 6
iteration= 19 chi2= 7061122.520741 time= 0.000572262 cumTime= 0.0132369 edges= 1402 schur= 0 lambda= 3776575690642662400.000000 levenbergIter= 3
direct method costs time: 0.0190083 seconds.
Tcw= 0.99546 0.094895 0.00739367 -0.428742
-0.0947181 0.995274 -0.0214247 0.236609
-0.00939182 0.0206271 0.999743 0.325207
0 0 0 1
*********** loop 7 ************
edges in graph: 1402
iteration= 0 chi2= 8437539.234503 time= 0.000910577 cumTime= 0.000910577 edges= 1402 schur= 0 lambda= 243676.298792 levenbergIter= 1
iteration= 1 chi2= 8389323.224253 time= 0.00064568 cumTime= 0.00155626 edges= 1402 schur= 0 lambda= 81225.432931 levenbergIter= 1
iteration= 2 chi2= 8301301.288900 time= 0.000643481 cumTime= 0.00219974 edges= 1402 schur= 0 lambda= 27075.144310 levenbergIter= 1
iteration= 3 chi2= 8139610.593232 time= 0.000642379 cumTime= 0.00284212 edges= 1402 schur= 0 lambda= 9025.048103 levenbergIter= 1
iteration= 4 chi2= 8092396.356961 time= 0.000647879 cumTime= 0.00349 edges= 1402 schur= 0 lambda= 3008.349368 levenbergIter= 1
iteration= 5 chi2= 8045445.410876 time= 0.000641909 cumTime= 0.0041319 edges= 1402 schur= 0 lambda= 1002.783123 levenbergIter= 1
iteration= 6 chi2= 8001048.609022 time= 0.000576728 cumTime= 0.00470863 edges= 1402 schur= 0 lambda= 334.261041 levenbergIter= 1
iteration= 7 chi2= 7973760.785844 time= 0.00051751 cumTime= 0.00522614 edges= 1402 schur= 0 lambda= 111.420347 levenbergIter= 1
iteration= 8 chi2= 7926942.921971 time= 0.000517125 cumTime= 0.00574327 edges= 1402 schur= 0 lambda= 37.140116 levenbergIter= 1
iteration= 9 chi2= 7908140.405514 time= 0.000521569 cumTime= 0.00626484 edges= 1402 schur= 0 lambda= 12.380039 levenbergIter= 1
iteration= 10 chi2= 7889394.652899 time= 0.000517242 cumTime= 0.00678208 edges= 1402 schur= 0 lambda= 4.126680 levenbergIter= 1
iteration= 11 chi2= 7851582.408679 time= 0.000516871 cumTime= 0.00729895 edges= 1402 schur= 0 lambda= 1.375560 levenbergIter= 1
iteration= 12 chi2= 7820763.314198 time= 0.000516679 cumTime= 0.00781563 edges= 1402 schur= 0 lambda= 0.458520 levenbergIter= 1
iteration= 13 chi2= 7813221.763484 time= 0.000508096 cumTime= 0.00832372 edges= 1402 schur= 0 lambda= 0.305680 levenbergIter= 1
iteration= 14 chi2= 7788387.024082 time= 0.000432315 cumTime= 0.00875604 edges= 1402 schur= 0 lambda= 0.101893 levenbergIter= 1
iteration= 15 chi2= 7757327.593048 time= 0.000435935 cumTime= 0.00919197 edges= 1402 schur= 0 lambda= 0.033964 levenbergIter= 1
iteration= 16 chi2= 7737954.680460 time= 0.000432867 cumTime= 0.00962484 edges= 1402 schur= 0 lambda= 0.011321 levenbergIter= 1
iteration= 17 chi2= 7706970.147861 time= 0.000432038 cumTime= 0.0100569 edges= 1402 schur= 0 lambda= 0.003774 levenbergIter= 1
iteration= 18 chi2= 7693565.230902 time= 0.000431903 cumTime= 0.0104888 edges= 1402 schur= 0 lambda= 0.001258 levenbergIter= 1
iteration= 19 chi2= 7681870.404585 time= 0.000431953 cumTime= 0.0109207 edges= 1402 schur= 0 lambda= 0.000419 levenbergIter= 1
iteration= 20 chi2= 7676432.816041 time= 0.000432092 cumTime= 0.0113528 edges= 1402 schur= 0 lambda= 0.000140 levenbergIter= 1
iteration= 21 chi2= 7671985.528550 time= 0.000432063 cumTime= 0.0117849 edges= 1402 schur= 0 lambda= 0.000047 levenbergIter= 1
iteration= 22 chi2= 7667366.173083 time= 0.000416933 cumTime= 0.0122018 edges= 1402 schur= 0 lambda= 0.000016 levenbergIter= 1
iteration= 23 chi2= 7662823.154996 time= 0.000371033 cumTime= 0.0125729 edges= 1402 schur= 0 lambda= 0.000005 levenbergIter= 1
iteration= 24 chi2= 7659343.840542 time= 0.00037062 cumTime= 0.0129435 edges= 1402 schur= 0 lambda= 0.000002 levenbergIter= 1
iteration= 25 chi2= 7659343.840542 time= 0.00127705 cumTime= 0.0142205 edges= 1402 schur= 0 lambda= 62170295762.145447 levenbergIter= 10
direct method costs time: 0.0205604 seconds.
Tcw= 0.995356 0.095103 -0.0148809 -0.340816
-0.0952447 0.995412 -0.00912524 0.207406
0.0139448 0.0105002 0.999848 0.365317
0 0 0 1
*********** loop 8 ************
edges in graph: 1402
iteration= 0 chi2= 8612213.046869 time= 0.000990377 cumTime= 0.000990377 edges= 1402 schur= 0 lambda= 180889.191265 levenbergIter= 1
iteration= 1 chi2= 8510649.250442 time= 0.000645923 cumTime= 0.0016363 edges= 1402 schur= 0 lambda= 60296.397088 levenbergIter= 1
iteration= 2 chi2= 8480848.275766 time= 0.000641791 cumTime= 0.00227809 edges= 1402 schur= 0 lambda= 20098.799029 levenbergIter= 1
iteration= 3 chi2= 8470569.083189 time= 0.000648903 cumTime= 0.00292699 edges= 1402 schur= 0 lambda= 6699.599676 levenbergIter= 1
iteration= 4 chi2= 8458735.042317 time= 0.000641074 cumTime= 0.00356807 edges= 1402 schur= 0 lambda= 2233.199892 levenbergIter= 1
iteration= 5 chi2= 8448058.215588 time= 0.000640722 cumTime= 0.00420879 edges= 1402 schur= 0 lambda= 744.399964 levenbergIter= 1
iteration= 6 chi2= 8424707.604723 time= 0.000629864 cumTime= 0.00483865 edges= 1402 schur= 0 lambda= 248.133321 levenbergIter= 1
iteration= 7 chi2= 8407773.249449 time= 0.00051538 cumTime= 0.00535403 edges= 1402 schur= 0 lambda= 82.711107 levenbergIter= 1
iteration= 8 chi2= 8388934.829240 time= 0.000520715 cumTime= 0.00587475 edges= 1402 schur= 0 lambda= 27.570369 levenbergIter= 1
iteration= 9 chi2= 8372101.766152 time= 0.000515195 cumTime= 0.00638994 edges= 1402 schur= 0 lambda= 9.190123 levenbergIter= 1
iteration= 10 chi2= 8359674.347141 time= 0.000515094 cumTime= 0.00690504 edges= 1402 schur= 0 lambda= 3.063374 levenbergIter= 1
iteration= 11 chi2= 8314001.542214 time= 0.000514859 cumTime= 0.0074199 edges= 1402 schur= 0 lambda= 1.021125 levenbergIter= 1
iteration= 12 chi2= 8269055.228943 time= 0.000514416 cumTime= 0.00793431 edges= 1402 schur= 0 lambda= 0.340375 levenbergIter= 1
iteration= 13 chi2= 8232415.617513 time= 0.000514405 cumTime= 0.00844872 edges= 1402 schur= 0 lambda= 0.113458 levenbergIter= 1
iteration= 14 chi2= 8203175.078067 time= 0.00043413 cumTime= 0.00888285 edges= 1402 schur= 0 lambda= 0.037819 levenbergIter= 1
iteration= 15 chi2= 8172279.427651 time= 0.000430993 cumTime= 0.00931384 edges= 1402 schur= 0 lambda= 0.012606 levenbergIter= 1
iteration= 16 chi2= 8141880.320204 time= 0.000430443 cumTime= 0.00974428 edges= 1402 schur= 0 lambda= 0.004202 levenbergIter= 1
iteration= 17 chi2= 8106376.512742 time= 0.000430813 cumTime= 0.0101751 edges= 1402 schur= 0 lambda= 0.001401 levenbergIter= 1
iteration= 18 chi2= 7951574.187300 time= 0.000430236 cumTime= 0.0106053 edges= 1402 schur= 0 lambda= 0.000467 levenbergIter= 1
iteration= 19 chi2= 7870008.835634 time= 0.000429854 cumTime= 0.0110352 edges= 1402 schur= 0 lambda= 0.000156 levenbergIter= 1
iteration= 20 chi2= 7843585.075304 time= 0.000428444 cumTime= 0.0114636 edges= 1402 schur= 0 lambda= 0.000052 levenbergIter= 1
iteration= 21 chi2= 7805648.041689 time= 0.000430766 cumTime= 0.0118944 edges= 1402 schur= 0 lambda= 0.000017 levenbergIter= 1
iteration= 22 chi2= 7760518.723738 time= 0.000440823 cumTime= 0.0123352 edges= 1402 schur= 0 lambda= 0.000006 levenbergIter= 1
iteration= 23 chi2= 7713627.879290 time= 0.000367868 cumTime= 0.0127031 edges= 1402 schur= 0 lambda= 0.000002 levenbergIter= 1
iteration= 24 chi2= 7666417.114061 time= 0.000367788 cumTime= 0.0130709 edges= 1402 schur= 0 lambda= 0.000001 levenbergIter= 1
iteration= 25 chi2= 7612743.598936 time= 0.000368045 cumTime= 0.0134389 edges= 1402 schur= 0 lambda= 0.000000 levenbergIter= 1
iteration= 26 chi2= 7586577.869319 time= 0.00036725 cumTime= 0.0138062 edges= 1402 schur= 0 lambda= 0.000000 levenbergIter= 1
iteration= 27 chi2= 7542643.224659 time= 0.000367743 cumTime= 0.0141739 edges= 1402 schur= 0 lambda= 0.000000 levenbergIter= 1
iteration= 28 chi2= 7507932.791750 time= 0.000368043 cumTime= 0.014542 edges= 1402 schur= 0 lambda= 0.000000 levenbergIter= 1
iteration= 29 chi2= 7453918.989195 time= 0.000370546 cumTime= 0.0149125 edges= 1402 schur= 0 lambda= 0.000000 levenbergIter= 1
direct method costs time: 0.0217702 seconds.
Tcw= 0.993005 0.091311 -0.0748564 0.0172835
-0.0893834 0.995583 0.028716 0.00386762
0.0771479 -0.0218242 0.996781 0.178906
0 0 0 1
*********** loop 9 ************
将点的大小变小后：
cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 2, cv::Scalar ( b,g,r ), 1 );
cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 2, cv::Scalar ( b,g,r ), 1 );
下面的是第二版的代码：
optical_flow.cpp
#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
/// Optical flow tracker and interface 光流跟踪
class OpticalFlowTracker {
public:
OpticalFlowTracker(
const Mat &img1_,//图像1
const Mat &img2_,//图像2
const vector<KeyPoint> &kp1_,//关键点1 -> 图像 1
vector<KeyPoint> &kp2_,//关键点2 -> 图像 2
vector<bool> &success_,//true if a keypoint is tracked successfully 关键点跟踪是正确的
bool inverse_ = true, bool has_initial_ = false) ://bool型变量 判断是否采用反向光流
img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
has_initial(has_initial_) {}
void calculateOpticalFlow(const Range &range);//定义calculateOpticalFlow（计算光流）函数
//Range中有两个关键的变量start和end Range可以用来表示矩阵的多个连续的行或列
//Range表示范围从start到end，包含start，但不包含end
private:
const Mat &img1;
const Mat &img2;
const vector<KeyPoint> &kp1;
vector<KeyPoint> &kp2;
vector<bool> &success;
bool inverse = true;
bool has_initial = false;
};
/**
* 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,//use inverse formulation?
bool has_initial_guess = false
);//定义OpticalFlowSingleLevel函数 单层光流法
/**
* 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
);//定义OpticalFlowMultiLevel 多层光流法
/**
* 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) // * 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表示内联函数，它是为了解决一些频繁调用的小函数大量消耗栈空间的问题而引入的
{
// boundary check(边界检验)
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= img.cols) x = img.cols - 1;
if (y >= img.rows) y = img.rows - 1;
uchar *data = &img.data[int(y) * img.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[img.step] +
xx * yy * data[img.step + 1]
);
}
int main(int argc, char **argv) {
// images, note they are CV_8UC1, not CV_8UC3
Mat img1 = imread(file_1, 0);//0表示返回灰度图
Mat img2 = imread(file_2, 0);//0表示返回灰度图
// key points, using GFTT here.
vector<KeyPoint> kp1;
Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
//GFTTDetector三个参数从左到右依次为
//maxCorners表示最大角点数目。在此处为500。
//qualityLevel表示角点可以接受的最小特征值，一般0.1或者0.01，不超过1。在此处为0.01。
//minDistance表示角点之间的最小距离。在此处为20。
detector->detect(img1, kp1);
// now lets track these key points in the second image
// first use single level LK in the validation picture
//利用OpenCV中的自带函数提取图像1中的GFTT角点
//然后利用calcOpticalFlowPyrLK()函数跟踪其在图像2中的位置(u,v)
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);//调用opencv OpticalFlowMultiLevel函数
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;//status中元素表示对应角点是否被正确跟踪到，1为正确跟踪，0为错误跟踪
vector<float> error; //error表示误差
t1 = chrono::steady_clock::now();//开始计时
cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);//调用opencv calcOpticalFlowPyrLK函数来求解min = || I1(x,y) - I2(x + δx, y + δy) ||2 视觉slam十四讲p214式8.10
t2 = chrono::steady_clock::now();//计时结束
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);//计算耗时
cout << "optical flow by opencv: " << time_used.count() << endl;//输出使用opencv函数计算光流的耗时
// plot the differences of those functions
Mat img2_single;//
cv::cvtColor(img2, img2_single, CV_GRAY2BGR);//将灰度图转换成彩色图，彩色图中BGR各颜色通道值为原先灰度值
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));
}
}
//画出角点连线图
Mat imgMatches(img1.rows, img1.cols * 2, CV_8UC1); //定义*行*列的Mat型变量
Rect rect1 = Rect(0, 0, img1.cols, img1.rows);
//Rect()有四个参数，第1个参数表示初始列，第2个参数表示初始行，
//第3个参数表示在初始列的基础上还要加上多少列（即矩形区域的宽度），第4个参数表示在初始行的基础上还要加上多少行（即矩形区域的高度）
Rect rect2 = Rect(img1.cols, 0, img2.cols, img2.rows);
img1.copyTo(imgMatches(rect1));
img2.copyTo(imgMatches(rect2));
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，并进行了初始化
OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
parallel_for_(Range(0, kp1.size()),
std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
//parallel_for_()实现并行调用OpticalFlowTracker::calculateOpticalFlow()的功能
}
//使用高斯牛顿法求解图像2中相应的角点坐标
void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
// parameters
int half_patch_size = 4;
int iterations = 10;//最大迭代次数
for (size_t i = range.start; i < range.end; i++)//对图像1中的每个GFTT角点进行高斯牛顿优化
{
auto kp = kp1[i];
double dx = 0, dy = 0; // dx,dy need to be estimated 优化变量
if (has_initial)//如果kp2进行了初始化，则执行
{
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 将H初始化为0
Eigen::Vector2d b = Eigen::Vector2d::Zero(); // bias 将b初始化为0
Eigen::Vector2d J; // jacobian 雅克比矩阵J
for (int iter = 0; iter < iterations; iter++) {
if (inverse == false)
{
H = Eigen::Matrix2d::Zero();
b = Eigen::Vector2d::Zero();
}
else
{
// only reset b 只重置矩阵b。在反向光流中，海塞矩阵H在整个高斯牛顿迭代过程中均保持不变
b = Eigen::Vector2d::Zero();
}
cost = 0;//代价初始化为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++) //x,y是patch内遍历
{
//(u, v)表示图像中的角点u表示x坐标，v表示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);//误差 eij = I1(u+i,v+j)-I2(U+I+Δu,v+j+Δv)
//i -> kp.pt.x
//j -> kp.pt.y
//u -> x
//v -> y
//Δu -> dx
//Δv -> 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))
);//dx,dy是优化变量 即（Δu，Δv） 计算雅克比矩阵
//相当于 J = - [ {I2( u + i + Δu + 1,v + j + Δv)-I2(u + i + Δu - 1,v + j + Δv)}/2,I2( u + i + Δu ,v + j + Δv + 1)-I2( u + i + Δu,v + j + Δv - 1)}/2]T T表示转置
//I2 -> 图像2的灰度信息
//u -> x
//v -> y
//Δu -> dx
//Δv -> dy
//i -> kp.pt.x
//j -> kp.pt.y
} 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))
);//dx,dy是优化变量 即（Δu，Δv） 计算雅克比矩阵
//相当于 J = - [ {I1( u + i + 1,v + j )-I1(u + i - 1,v + j )}/2,I1( u + i,v + j + 1)-I1( u + i ,v + j - 1)}/2]T T表示转置
//I2 -> 图像2的灰度信息
//i -> x
//j -> y
//u -> kp.pt.x
//v -> kp.pt.y
}
// compute H, b and set cost;
b += -error * J;//b = -Jij * eij(累加和)
cost += error * error;//cost = || eij ||2 2范数
if (inverse == false || iter == 0) {
// also update H
H += J * J.transpose();//H = Jij Jij(T)(累加和)
}
}
// compute update
//求解增量方程，计算更新量
Eigen::Vector2d update = H.ldlt().solve(b);
if (std::isnan(update[0]))//计算出来的更新量是非数字，光流跟踪失败，退出GN迭代
{
// 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) //代价不再减小，退出GN迭代
{
break;
}
// update dx, dy 更新优化变量和lastCost
dx += update[0];
dy += update[1];
lastCost = cost;
succ = true;
if (update.norm() < 1e-2) //更新量的模小于1e-2，退出GN迭代
{
// converge
break;
}
}//GN法进行完一次迭代
success[i] = succ;
// set kp2
kp2[i].pt = kp.pt + Point2f(dx, dy);
}
}//对图像1中的所有角点都完成了光流跟踪
void OpticalFlowMultiLevel(
const Mat &img1,
const Mat &img2,
const vector<KeyPoint> &kp1,
vector<KeyPoint> &kp2,
vector<bool> &success,
bool inverse) {
// parameters
int pyramids = 4;//金字塔层数为4
double pyramid_scale = 0.5;//每层之间的缩放因子设为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 pyr1 -> 图像1的金字塔 pyr2 -> 图像2的金字塔
for (int i = 0; i < pyramids; i++) {
if (i == 0)
{
pyr1.push_back(img1);
pyr2.push_back(img2);
}
else
{
Mat img1_pyr, img2_pyr;
//将图像pyr1[i-1]的宽和高各缩放0.5倍得到图像img1_pyr
cv::resize(pyr1[i - 1], img1_pyr,
cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
//将图像pyr2[i-1]的宽和高各缩放0.5倍得到图像img2_pyr
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;//这里意思大概是视觉slam十四讲p215的把上一层的追踪结果作为下一层光流的初始值
kp_top.pt *= scales[pyramids - 1];//
kp1_pyr.push_back(kp_top);//最顶层图像1的角点坐标
kp2_pyr.push_back(kp_top);//最顶层图像2的角点坐标：用图像1的初始化图像2的
}
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);
//has_initial设置为true，表示图像2中的角点kp2_pyr进行了初始化
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;//pyramidScale等于0.5，相当于乘了2
for (auto &kp: kp2_pyr)
kp.pt /= pyramid_scale;//pyramidScale等于0.5，相当于乘了2
}
}
for (auto &kp: kp2_pyr)
kp2.push_back(kp);//存输出kp2
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ch8)
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 ${SSE_FLAGS} -g -O3 -march=native")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED)
find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
"/usr/include/eigen3/"
${Pangolin_INCLUDE_DIRS}
)
add_executable(optical_flow optical_flow.cpp)
target_link_libraries(optical_flow ${OpenCV_LIBS} fmt)
add_executable(direct_method direct_method.cpp)
target_link_libraries(direct_method ${OpenCV_LIBS} ${Pangolin_LIBRARIES} fmt)
执行结果：
./optical_flow
build pyramid time: 0.000126517
track pyr 3 cost time: 0.000207429
track pyr 2 cost time: 0.000199243
track pyr 1 cost time: 0.000172433
track pyr 0 cost time: 0.000155421
optical flow by gauss-newton: 0.000939911
optical flow by opencv: 0.00107912
direct_method.cpp
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>
using namespace std;
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
// Camera intrinsics 相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline 双目相机基线
double baseline = 0.573;
// paths 图像路径
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png"); // other files
// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
/// class for accumulator jacobians in parallel 用于并行计算雅可比矩阵的类
class JacobianAccumulator {
public:
//类的构造函数，使用列表进行初始化
JacobianAccumulator(
const cv::Mat &img1_,
const cv::Mat &img2_,
const VecVector2d &px_ref_,//角点坐标
const vector<double> depth_ref_,//路标点的Z坐标值
Sophus::SE3d &T21_) :
img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
}
/// accumulate jacobians in a range 在range范围内加速计算雅可比矩阵
void accumulate_jacobian(const cv::Range &range);
/// get hessian matrix 获取海塞矩阵
Matrix6d hessian() const { return H; }
/// get bias 获取矩阵b
Vector6d bias() const { return b; }
/// get total cost 获取总共的代价
double cost_func() const { return cost; }
/// get projected points 获取图像2中的角点坐标
VecVector2d projected_points() const { return projection; }
/// reset h, b, cost to zero 将海塞矩阵H，矩阵b和代价cost置为0
void reset() {
H = Matrix6d::Zero();
b = Vector6d::Zero();
cost = 0;
}
private:
const cv::Mat &img1;
const cv::Mat &img2;
const VecVector2d &px_ref;//图像1中角点坐标
const vector<double> depth_ref;//图像1中路标点的Z坐标值
Sophus::SE3d &T21;
VecVector2d projection; // projected points
std::mutex hessian_mutex;
Matrix6d H = Matrix6d::Zero();
Vector6d b = Vector6d::Zero();
double cost = 0;
};
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T21
*/
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21
);
//定义DirectPoseEstimationMultiLayer函数 多层直接法
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T21
*/
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21
);
//定义DirectPoseEstimationSingleLayer函数 单层直接法
// bilinear interpolation 双线性插值求灰度值
inline float GetPixelValue(const cv::Mat &img, float x, float y) //inline表示内联函数，它是为了解决一些频繁调用的小函数大量消耗栈空间的问题而引入的
{
// boundary check
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= img.cols) x = img.cols - 1;
if (y >= img.rows) y = img.rows - 1;
//...|I1 I2|...
//...| |...
//...| |...
//...|I3 I4|...
uchar *data = &img.data[int(y) * img.step + int(x)];//x和y是整数
//data[0] -> I1 data[1] -> I2 data[img.step] -> I3 data[img.step + 1] -> I4
float xx = x - floor(x);//xx算出的是x的小数部分
float yy = y - floor(y);//yy算出的是y的小数部分
return float//最终的像素灰度值
(
(1 - xx) * (1 - yy) * data[0] +
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
int main(int argc, char **argv) {
cv::Mat left_img = cv::imread(left_file, 0);//0表示返回灰度图，left.png表示000000.png
cv::Mat disparity_img = cv::imread(disparity_file, 0);//0表示返回灰度图，disparity.png是left.png的视差图
// let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
//在图像1中随机选择一些像素点，然后恢复深度，得到一些路标点
cv::RNG rng;
int nPoints = 2000;
int boarder = 20;
VecVector2d pixels_ref;
vector<double> depth_ref;
// generate pixels in ref and load depth data
for (int i = 0; i < nPoints; i++) {
int x = rng.uniform(boarder, left_img.cols - boarder); // don't pick pixels close to boarder 不要拾取靠近边界的像素
int y = rng.uniform(boarder, left_img.rows - boarder); // don't pick pixels close to boarder 不要拾取靠近边界的像素
int disparity = disparity_img.at<uchar>(y, x);
double depth = fx * baseline / disparity; // you know this is disparity to depth
depth_ref.push_back(depth);
pixels_ref.push_back(Eigen::Vector2d(x, y));
}
// estimates 01~05.png's pose using this information
Sophus::SE3d T_cur_ref;
for (int i = 1; i < 6; i++)// 1~5 i从1到5，共5张图
{
cv::Mat img = cv::imread((fmt_others % i).str(), 0);//读取图片，0表示返回一张灰度图
// try single layer by uncomment this line
// DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
//利用单层直接法计算图像img相对于left_img的位姿T_cur_ref，以图片left.png为基准
DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);//调用DirectPoseEstimationMultiLayer 多层直接法
}
return 0;
}
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,//第1张图中的角点坐标
const vector<double> depth_ref,//第1张图中路标点的Z坐标值 就是深度信息
Sophus::SE3d &T21) {
const int iterations = 10;//设置迭代次数为10
double cost = 0, lastCost = 0;//将代价和最终代价初始化为0
auto t1 = chrono::steady_clock::now();//开始计时
JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);
for (int iter = 0; iter < iterations; iter++) {
jaco_accu.reset();//重置
//完成并行计算海塞矩阵H，矩阵b和代价cost
cv::parallel_for_(cv::Range(0, px_ref.size()),
std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
Matrix6d H = jaco_accu.hessian();//计算海塞矩阵
Vector6d b = jaco_accu.bias();//计算b矩阵
// solve update and put it into estimation
//求解增量方程更新优化变量T21
Vector6d update = H.ldlt().solve(b);;
T21 = Sophus::SE3d::exp(update) * T21;
cost = jaco_accu.cost_func();
if (std::isnan(update[0])) //解出来的更新量不是一个数字，退出迭代
{
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost) //代价不再减小，退出迭代
{
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
if (update.norm() < 1e-3) //更新量的模小于1e-3，退出迭代
{
// converge
break;
}
lastCost = cost;
cout << "iteration: " << iter << ", cost: " << cost << endl;
}//GN(高斯牛顿法)迭代结束
cout << "T21 = \n" << T21.matrix() << endl;//输出T21矩阵
auto t2 = chrono::steady_clock::now();//计时结束
auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);//计算耗时
cout << "direct method for single layer: " << time_used.count() << endl;//输出使用单层直接法所用时间
// plot the projected pixels here
cv::Mat img2_show;
cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
VecVector2d projection = jaco_accu.projected_points();
for (size_t i = 0; i < px_ref.size(); ++i) {
auto p_ref = px_ref[i];
auto p_cur = projection[i];
if (p_cur[0] > 0 && p_cur[1] > 0) {
cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
cv::Scalar(0, 250, 0));
}
}
cv::imshow("current", img2_show);
cv::waitKey();
}
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {
// parameters
const int half_patch_size = 1;
int cnt_good = 0;
Matrix6d hessian = Matrix6d::Zero();
Vector6d bias = Vector6d::Zero();
double cost_tmp = 0;
for (size_t i = range.start; i < range.end; i++) {
// compute the projection in the second image //point_ref表示图像1中的路标点
Eigen::Vector3d point_ref =
depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
Eigen::Vector3d point_cur = T21 * point_ref;//point_cur表示图像2中的路标点
if (point_cur[2] < 0) // depth invalid
continue;
//u,v表示图像2中对应的角点坐标
float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;//视觉slam十四讲p99式5.5
// u = fx * X / Z + cx v = fy * Y / Z + cy X -> point_cur[0] Y -> point_cur[1] Z -> point_cur[2]
if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
v > img2.rows - half_patch_size)
continue;
projection[i] = Eigen::Vector2d(u, v);//projection表示图像2中相应的角点坐标值
double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;// Z2_inv = (1 / (Z * Z))
cnt_good++;
// and compute error and jacobian 计算海塞矩阵H，矩阵b和代价cost
for (int x = -half_patch_size; x <= half_patch_size; x++)
for (int y = -half_patch_size; y <= half_patch_size; y++) {
//ei = I1(p1,i) - I(p2,i)其中p1，p2空间点P在两个时刻的像素位置坐标
double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
GetPixelValue(img2, u + x, v + y);//视觉slam十四讲p219式8.13
Matrix26d J_pixel_xi;
Eigen::Vector2d J_img_pixel;
//视觉slam十四讲p220式8.18
J_pixel_xi(0, 0) = fx * Z_inv;
J_pixel_xi(0, 1) = 0;
J_pixel_xi(0, 2) = -fx * X * Z2_inv;
J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
J_pixel_xi(0, 5) = -fx * Y * Z_inv;
J_pixel_xi(1, 0) = 0;
J_pixel_xi(1, 1) = fy * Z_inv;
J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
J_pixel_xi(1, 5) = fy * X * Z_inv;
// -fx * inv_z 相当于-fx / Z
//0
// -fx * X * Z2_inv相当于fx * X / ( Z * Z )
//--fx * X * Y * Z2_inv相当于fx * X * Y / ( Z * Z) +fx
//fx + fx * X * X * Z2_inv相当于fx * X * X / (Z * Z)
//-fx * Y * Z_inv相当于 fx * Y / Z
//0
//fy * Z_inv相当于-fy / Z
//-fy * Y * Z2_inv相当于fy * Y / (Z * Z)
//-fy - fy * Y * Y * Z2_inv相当于fy + fy * Y * Y / (Z * Z)
//fy * X * Y * Z2_inv相当于fy * X * Y / ( Z * Z)
//fy * X * Z_inv相当于-fy * X / Z
J_img_pixel = Eigen::Vector2d(
0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
);//dx,dy是优化变量 即（Δu，Δv） 计算雅克比矩阵
//dx,dy是优化变量 即（Δu，Δv） 计算雅克比矩阵
//相当于 J = - [ {I1( u + i + 1,v + j )-I1(u + i - 1,v + j )}/2,I1( u + i,v + j + 1)-I1( u + i ,v + j - 1)}/2]T T表示转置
//I1 -> 图像1的灰度信息
//i -> x
//j -> y
// total jacobian
Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();
hessian += J * J.transpose();
bias += -error * J;
cost_tmp += error * error;
}
}
if (cnt_good) {
// set hessian, bias and cost
unique_lock<mutex> lck(hessian_mutex);
H += hessian;//H = Jij Jij(T)(累加和)
b += bias;//b = -Jij * eij(累加和)
cost += cost_tmp / cnt_good;//cost = || eij ||2 2范数
}
}
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3d &T21) {
// parameters
int pyramids = 4;//金字塔层数为4
double pyramid_scale = 0.5;//每层之间的缩放因子设为0.5
double scales[] = {1.0, 0.5, 0.25, 0.125};
// create pyramids 创建图像金字塔
vector<cv::Mat> pyr1, pyr2; // image pyramids pyr1 -> 图像1的金字塔 pyr2 -> 图像2的金字塔
for (int i = 0; i < pyramids; i++) {
if (i == 0) {
pyr1.push_back(img1);
pyr2.push_back(img2);
} else {
cv::Mat img1_pyr, img2_pyr;
//将图像pyr1[i-1]的宽和高各缩放0.5倍得到图像img1_pyr
cv::resize(pyr1[i - 1], img1_pyr,
cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
//将图像pyr2[i-1]的宽和高各缩放0.5倍得到图像img2_pyr
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);
}
}
double fxG = fx, fyG = fy, cxG = cx, cyG = cy; // backup the old values 备份旧值
for (int level = pyramids - 1; level >= 0; level--) {
VecVector2d px_ref_pyr; // set the keypoints in this pyramid level 设置此金字塔级别中的关键点
for (auto &px: px_ref) {
px_ref_pyr.push_back(scales[level] * px);
}
// scale fx, fy, cx, cy in different pyramid levels 在不同的金字塔级别缩放 fx, fy, cx, cy
fx = fxG * scales[level];
fy = fyG * scales[level];
cx = cxG * scales[level];
cy = cyG * scales[level];
DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
}
}
CMakeLists.txt
和上面一样
执行结果：
./direct_methoditeration: 0, cost: 2.86149e+06
iteration: 1, cost: 1.24891e+06
iteration: 2, cost: 502096
iteration: 3, cost: 309320
cost increased: 323860, 309320
T21 =
0.999991 0.00230082 0.00354082 -0.00342245
-0.00231019 0.999994 0.00264471 0.00141269
-0.00353471 -0.00265286 0.99999 -0.727667
0 0 0 1
direct method for single layer: 0.00788667
iteration: 0, cost: 342987
cost increased: 350341, 342987
T21 =
0.999989 0.00304081 0.00346467 0.0013507
-0.00304864 0.999993 0.0022568 0.00629644
-0.00345778 -0.00226734 0.999991 -0.729185
0 0 0 1
direct method for single layer: 0.00129193
iteration: 0, cost: 485588
iteration: 1, cost: 433949
cost increased: 437925, 433949
T21 =
0.999991 0.00251354 0.00346628 -0.00271413
-0.00252164 0.999994 0.00233523 0.00243139
-0.00346039 -0.00234395 0.999991 -0.734721
0 0 0 1
direct method for single layer: 0.00153717
iteration: 0, cost: 671749
T21 =
0.999991 0.00248083 0.00343393 -0.00374053
-0.00248837 0.999994 0.00219446 0.00304549
-0.00342847 -0.00220299 0.999992 -0.732343
0 0 0 1
direct method for single layer: 0.00123316
iteration: 0, cost: 2.51555e+06
iteration: 1, cost: 1.71524e+06
iteration: 2, cost: 1.14561e+06
iteration: 3, cost: 716082
iteration: 4, cost: 514530
iteration: 5, cost: 464441
cost increased: 469800, 464441
T21 =
0.99997 0.000890844 0.0076648 0.00626557
-0.000926754 0.999989 0.00468276 0.000485719
-0.00766054 -0.00468973 0.99996 -1.46297
0 0 0 1
direct method for single layer: 0.00228563
iteration: 0, cost: 669221
iteration: 1, cost: 647188
cost increased: 656070, 647188
T21 =
0.99997 0.00111814 0.00762127 0.0036341
-0.00114909 0.999991 0.00405769 0.00355783
-0.00761666 -0.00406633 0.999963 -1.47096
0 0 0 1
direct method for single layer: 0.00172928
iteration: 0, cost: 813344
iteration: 1, cost: 794647
cost increased: 801590, 794647
T21 =
0.999971 0.000722536 0.00764427 -0.000370313
-0.00075261 0.999992 0.00393214 0.00253998
-0.00764137 -0.00393778 0.999963 -1.48186
0 0 0 1
direct method for single layer: 0.00158673
iteration: 0, cost: 929422
iteration: 1, cost: 886157
cost increased: 888568, 886157
T21 =
0.999971 0.000697443 0.00759024 -0.00250787
-0.000725732 0.999993 0.00372495 0.0039643
-0.00758759 -0.00373035 0.999964 -1.48135
0 0 0 1
direct method for single layer: 0.00136832
iteration: 0, cost: 2.41726e+06
iteration: 1, cost: 1.99178e+06
iteration: 2, cost: 1.6102e+06
iteration: 3, cost: 1.4033e+06
iteration: 4, cost: 1.11569e+06
iteration: 5, cost: 947643
iteration: 6, cost: 747482
iteration: 7, cost: 693225
cost increased: 693749, 693225
T21 =
0.999941 0.00147946 0.0107455 0.0385259
-0.00154019 0.999983 0.00564533 0.0115181
-0.010737 -0.00566155 0.999926 -2.18525
0 0 0 1
direct method for single layer: 0.00290631
iteration: 0, cost: 900638
iteration: 1, cost: 853208
iteration: 2, cost: 842470
cost increased: 853931, 842470
T21 =
0.999937 0.001409 0.0111432 0.0256605
-0.0014693 0.999984 0.00540533 0.00282104
-0.0111354 -0.00542136 0.999923 -2.2155
0 0 0 1
direct method for single layer: 0.00209442
iteration: 0, cost: 1.14623e+06
iteration: 1, cost: 1.10642e+06
iteration: 2, cost: 1.10107e+06
cost increased: 1.10398e+06, 1.10107e+06
T21 =
0.999935 0.0015245 0.0112762 0.0182413
-0.00158631 0.999984 0.00547507 -0.00501183
-0.0112677 -0.0054926 0.999921 -2.23479
0 0 0 1
direct method for single layer: 0.00192249
iteration: 0, cost: 1.61177e+06
iteration: 1, cost: 1.51201e+06
iteration: 2, cost: 1.47355e+06
cost increased: 1.47477e+06, 1.47355e+06
T21 =
0.999934 0.00126182 0.0114042 0.00305559
-0.0013221 0.999985 0.00527994 -0.000967677
-0.0113974 -0.00529467 0.999921 -2.24112
0 0 0 1
direct method for single layer: 0.00195658
iteration: 0, cost: 2.6218e+06
iteration: 1, cost: 2.31544e+06
iteration: 2, cost: 1.89746e+06
iteration: 3, cost: 1.6753e+06
iteration: 4, cost: 1.39565e+06
iteration: 5, cost: 1.19339e+06
iteration: 6, cost: 1.0917e+06
iteration: 7, cost: 1.01995e+06
iteration: 8, cost: 940921
iteration: 9, cost: 934156
T21 =
0.999872 -0.000262248 0.0159995 0.0245805
0.000147523 0.999974 0.00717126 -0.0040278
-0.016001 -0.00716798 0.999846 -2.9621
0 0 0 1
direct method for single layer: 0.00321337
iteration: 0, cost: 1.13276e+06
iteration: 1, cost: 1.09144e+06
iteration: 2, cost: 1.09026e+06
cost increased: 1.09104e+06, 1.09026e+06
T21 =
0.999866 -0.000288411 0.0163843 0.0124827
0.000179441 0.999978 0.00665199 -0.00512523
-0.0163858 -0.00664816 0.999844 -3.00389
0 0 0 1
direct method for single layer: 0.00154216
iteration: 0, cost: 1.4759e+06
iteration: 1, cost: 1.46226e+06
iteration: 2, cost: 1.44894e+06
cost increased: 1.44984e+06, 1.44894e+06
T21 =
0.999865 -0.000195523 0.0164373 0.00279762
9.26072e-05 0.99998 0.00626168 -0.00473162
-0.0164382 -0.00625932 0.999845 -3.01713
0 0 0 1
direct method for single layer: 0.00152807
iteration: 0, cost: 2.22582e+06
iteration: 1, cost: 2.15677e+06
cost increased: 2.21724e+06, 2.15677e+06
T21 =
0.999864 0.00017151 0.0164623 -0.00920258
-0.000268269 0.999983 0.00587555 0.000393268
-0.016461 -0.00587917 0.999847 -3.02448
0 0 0 1
direct method for single layer: 0.00173756
iteration: 0, cost: 2.88843e+06
iteration: 1, cost: 2.63136e+06
iteration: 2, cost: 2.19455e+06
iteration: 3, cost: 1.76206e+06
iteration: 4, cost: 1.4942e+06
iteration: 5, cost: 1.33871e+06
iteration: 6, cost: 1.28744e+06
iteration: 7, cost: 1.21232e+06
iteration: 8, cost: 1.20579e+06
iteration: 9, cost: 1.19902e+06
T21 =
0.999802 0.000604608 0.0198866 0.0405523
-0.000748414 0.999974 0.00722462 0.0108371
-0.0198817 -0.00723808 0.999776 -3.76346
0 0 0 1
direct method for single layer: 0.00216502
iteration: 0, cost: 1.75298e+06
iteration: 1, cost: 1.70448e+06
iteration: 2, cost: 1.6791e+06
iteration: 3, cost: 1.65447e+06
iteration: 4, cost: 1.65178e+06
iteration: 5, cost: 1.56388e+06
T21 =
0.999783 0.000712451 0.0208365 0.00333502
-0.000854239 0.999977 0.00679664 0.00853946
-0.0208312 -0.00681297 0.99976 -3.83408
0 0 0 1
direct method for single layer: 0.00272291
iteration: 0, cost: 2.31197e+06
iteration: 1, cost: 2.1256e+06
cost increased: 2.19484e+06, 2.1256e+06
T21 =
0.999778 0.00103877 0.0210486 -0.00750971
-0.00117076 0.99998 0.0062593 0.0117474
-0.0210416 -0.00628255 0.999759 -3.85214
0 0 0 1
direct method for single layer: 0.00114812
iteration: 0, cost: 2.94452e+06
cost increased: 2.97505e+06, 2.94452e+06
T21 =
0.999786 0.00105871 0.0206453 -0.00288281
-0.00118441 0.999981 0.00607705 0.0108692
-0.0206385 -0.00610021 0.999768 -3.85721
0 0 0 1
direct method for single layer: 0.000892443
1. 除了LK 光流之外，还有哪些光流方法？它们各有什么特点？
还有KLT、HS、LK等光流。
KLT光流公式推导请参考：
https://www.cnblogs.com/mthoutai/p/7150625.html有兴趣的可以研究一下具体的代码：
链接：https://pan.baidu.com/s/1P0jZiXtu3IbnP1EtMzAVfQ
提取码：8888
2. 在本节的程序的求图像梯度过程中，我们简单地求了u+1 和u-1 的灰度之差除2，作为u 方向上的梯度值。这种做法有什么缺点？提示：对于距离较近的特征，变化应该较快；而距离较远的特征在图像中变化较慢，求梯度时能否利用此信息？
参考这篇文章：
3. 直接法能否和光流一样，提出反向法的概念？即，使用原始图像的梯度代替目标图像的梯度。
参考下面的：
视觉SLAM十四讲CH8课后习题3、4_nudt一枚研究生-CSDN博客
https://blog.csdn.net/weixin_53660567/article/details/121274478
4. *使用Ceres 实现RGB-D 上的稀疏直接法和半稠密直接法。
视觉SLAM十四讲CH8课后习题3、4_nudt一枚研究生-CSDN博客https://blog.csdn.net/weixin_53660567/article/details/121274478
5. 相比于RGB-D 的直接法，单目直接法往往更加复杂。除了匹配未知之外，像素的距离也是待估计的。我们需要在优化时把像素深度也作为优化变量。阅读[57, 59]，你能理解它的原理吗？如果不能，请在13 讲之后再回来阅读。
*************
947
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
