视觉SLAM十四讲课后习题—ch8

1.除了LK光流之外,还有哪些光流方法?它们各有什么特点?

  参考:https://blog.csdn.net/on2way/article/details/48953975

1)LK(Lucas-Kanada)光流: 一种经典的方法,基于局部特征计算光流
2)Horn-Schunck光流方法:全局性质下计算光流的方法,该方法运算较慢
3)Buxton方法:一种基于边缘运动的图像序列模型的方法
4)Black-Jepson方法

 

2.在本节程序的求图像梯度过程中,我们简单地求了u+1和u-1的灰度之差处以2,作为u方向上的梯度值。这种做法有什么缺点?提示:对于距离较近的特征,变化应该较快,而距离较远的特征在图像中变化较慢,求梯度时能否利用此信息?

  本节程序中求图像梯度的过程:

 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;

 

  显然,当灰度值变化很快的时候,u方向上的梯度值是不能等于u+1处的梯度值和u-1处的梯度值之差除以2的。

 

3.在稀疏直接法中,假设单个像素周围小块的光度也不变,是否可以提高算法的健壮性,请编程实现。

  分析:直接法是基于灰度不变假设,即同一个空间点的像素灰度值在各个图像中是固定不变的。如果现在假设单个像素周围小块的光度也不变,我们就可以用单个像素周围小块的平均灰度值来代替单个像素的灰度值。至于是否能提高健壮性,还需实验和评估。

  类似的思想见ch13讲中的块匹配

  在稀疏直接法实践(见《视觉SLAM十四讲》笔记(ch8)2.1实践)的基础上更改程序:

  参考:https://www.cnblogs.com/newneul/p/8571653.html    

  1 #include <iostream>
  2 #include <fstream>
  3 #include <list>
  4 #include <vector>
  5 #include <chrono>
  6 #include <ctime>
  7 #include <climits>
  8 
  9 #include <opencv2/core/core.hpp>
 10 #include <opencv2/imgproc/imgproc.hpp>
 11 #include <opencv2/highgui/highgui.hpp>
 12 #include <opencv2/features2d/features2d.hpp>
 13 
 14 #include <g2o/core/base_unary_edge.h>
 15 #include <g2o/core/block_solver.h>
 16 #include <g2o/core/optimization_algorithm_levenberg.h>
 17 #include <g2o/solvers/dense/linear_solver_dense.h>
 18 #include <g2o/core/robust_kernel.h>
 19 #include <g2o/types/sba/types_six_dof_expmap.h>
 20 
 21 using namespace std;
 22 using namespace g2o;
 23 
 24 int patchRadius=1;
 25 //获取小块平均灰度值
 26 // gray:灰度矩阵 x,y表示以(x,y)为中心
 27 //计算的小块的平均灰度
 28 float getPatchAverageGray(const cv::Mat &gray ,float u, float v);
 29 
 30 
 31 struct Measurement
 32 {
 33     Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
 34     Eigen::Vector3d pos_world;
 35     float grayscale;
 36 };
 37 
 38 //转换成相机坐标系下坐标
 39 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
 40 {
 41     float zz = float ( d ) /scale;
 42     float xx = zz* ( x-cx ) /fx;
 43     float yy = zz* ( y-cy ) /fy;
 44     return Eigen::Vector3d ( xx, yy, zz );
 45 }
 46 
 47 inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
 48 {
 49     float u = fx*x/z+cx;
 50     float v = fy*y/z+cy;
 51     return Eigen::Vector2d ( u,v );
 52 }
 53 
 54 // 直接法估计位姿
 55 // 输入:测量值(空间点的灰度),新的灰度图,相机内参;
 56 //输出:相机位姿
 57 void poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
 58 
 59 // project a 3d point into an image plane, the error is photometric error
 60 // an unary edge with one vertex SE3Expmap (the pose of camera)
 61                                             //误差值维度 误差类型 顶点类型
 62 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
 63 {
 64 public:
 65     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 66 
 67     EdgeSE3ProjectDirect() = default; //代替下面的方式 默认产生合成的构造函数
 68     //EdgeSE3ProjectDirect(){}
 69     EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
 70         : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )      //灰度图像指针
 71     {}
 72 
 73     virtual void computeError()
 74     {
 75         const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
 76         Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
 77         float x = x_local[0]*fx_/x_local[2] + cx_;  //世界坐标转换到当期帧像素坐标
 78         float y = x_local[1]*fy_/x_local[2] + cy_;
 79         // check x,y is in the image
 80         //距离图像四条边4个像素大小的区域内作为有效投影区域 对于不在该范围内的点误差值设为0 为了防止计算的误差太大 拉低内点对误差的影响 导致估计的RT严重偏离真值
 81         if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
 82         {
 83             _error ( 0,0 ) = 0.0;
 84             this->setLevel ( 1 );
 85         }
 86         else
 87         {
 88             float sumValue = 0.0;
 89             for(int i = x-1; i<=x+1 ; ++i)
 90                 for (int j = y-1; j <= y+1 ; ++j) {
 91                     sumValue += getPixelValue(i,j);
 92                 }
 93             //sumValue /=( (2*PATCH_RADIUS +1)*(2*PATCH_RADIUS+1) );  //求得元素周围小块的平均灰度值
 94        sumValue/=9;
 95             _error (0,0) = sumValue - _measurement;
 96         }
 97     }
 98 
 99     // plus in manifold
100     //提供误差关于位姿的雅克比矩阵 书上8.16式子 只不过负号去掉了 因为用的是当前帧灰度值 - 世界坐标下的测量值
101     virtual void linearizeOplus( )override
102     {
103         if ( level() == 1 )
104         {
105             _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
106             return;
107         }
108         VertexSE3Expmap* vtx = dynamic_cast<VertexSE3Expmap*> ( _vertices[0] );
109         Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book 转换到第二帧坐标系下
110 
111         double x = xyz_trans[0];
112         double y = xyz_trans[1];
113         double invz = 1.0/xyz_trans[2];
114         double invz_2 = invz*invz;
115 
116         float u = x*fx_*invz + cx_;//投影到第二帧像素坐标系
117         float v = y*fy_*invz + cy_;
118 
119         // jacobian from se3 to u,v
120         // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
121         Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
122 
123         //书上8.15式子
124         jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
125         jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
126         jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
127         jacobian_uv_ksai ( 0,3 ) = invz *fx_;
128         jacobian_uv_ksai ( 0,4 ) = 0;
129         jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
130 
131         jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
132         jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
133         jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
134         jacobian_uv_ksai ( 1,3 ) = 0;
135         jacobian_uv_ksai ( 1,4 ) = invz *fy_;
136         jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
137 
138         Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
139 
140         //书上I2对像素坐标系的偏导数  这里很有可能 计算出来的梯度为0  因为FAST角点的梯度没有限制
141         //这也是半稠密法主要改进的地方 就是选关键点的时候 选择梯度大的点 因此这里的梯度就不可能为0了
142         jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
143         jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
144 
145         _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//书上8.16式子
146     }
147 
148     // dummy read and write functions because we don't care...
149     virtual bool read ( std::istream& in ) {}
150     virtual bool write ( std::ostream& out ) const {}
151 
152 protected:
153     // get a gray scale value from reference image (bilinear interpolated)
154     //下面的方式 针对单通道的灰度图
155     inline float getPixelValue ( float x, float y )//通过双线性插值获取浮点坐标对应的插值后的像素值
156     {
157         uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];//step表示图像矩阵一行的所有字节(包括所有通道的总和),data表示存储图像的开始指针
158         float xx = x - floor ( x );     //取整函数
159         float yy = y - floor ( y );
160         return float (  //公式f(i+u,j+v) = (1-u)(1-v)f(i,j) + u(1-v)f(i+1,j) + (1-u)vf(i,j+1) + uvf(i+1,j+1)
161                         //这里的xx 就是u  yy就是v
162                    ( 1-xx ) * ( 1-yy ) * data[0] +
163                    xx* ( 1-yy ) * data[1] +
164                    ( 1-xx ) *yy*data[ image_->step ] +  //I(i+1,j) //这里相当于像素的周期是image_->step,即每一行存储像素的个数为image_->step
165                    xx*yy*data[image_->step+1]  //I(i+1,j+1)        //data[image_->step]是I(i,j)对应的下一行像素为I(i+1,j)
166                );
167     }
168 public:
169     Eigen::Vector3d x_world_;   // 3D point in world frame
170     float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
171     cv::Mat* image_=nullptr;    // reference image
172 };
173 
174 int main ( int argc, char** argv )
175 {
176     if ( argc != 2 )
177     {
178         cout<<"usage: useLK path_to_dataset"<<endl;
179         return 1;
180     }
181     srand ( ( unsigned int ) time ( 0 ) );
182     string path_to_dataset = argv[1];
183     string associate_file = path_to_dataset + "/associate.txt";
184 
185     ifstream fin ( associate_file );
186 
187     string rgb_file, depth_file, time_rgb, time_depth;
188     cv::Mat color, depth, gray;
189     vector<Measurement> measurements;//Measurement类 存储世界坐标点(以第一帧为参考的FAST关键点) 和 对应的灰度图像(由color->gray)的灰度值
190     // 相机内参
191     float cx = 325.5;
192     float cy = 253.5;
193     float fx = 518.0;
194     float fy = 519.0;
195     float depth_scale = 1000.0;
196     Eigen::Matrix3f K;
197     K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
198 
199     Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//三维变换矩阵T 4X4 初始时刻是单位R矩阵+0平移向量
200 
201     cv::Mat prev_color;
202     // 我们以第一个图像为参考,对后续图像和参考图像做直接法 ,每一副图像 都会与第一帧图像做直接法计算第一帧到当前帧的RT 但是经过更多的帧后 关键点的数量会减少,
203     //所以实际应用时 应当规定关键点的数量少于多少 就该从新设定参考系,再次利用直接法 ,但是会累计的误差需要解决????
204     for ( int index=0; index<10; index++ )//总共10帧
205     {
206         cout<<"*********** loop "<<index<<" ************"<<endl;
207         fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
208         color = cv::imread ( path_to_dataset+"/"+rgb_file );
209         depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );//-1 按原图像的方式存储 detph 16位存储
210         if ( color.data==nullptr || depth.data==nullptr )
211             continue;
212         //转换后的灰度图为g2o优化需要的边提供灰度值
213         cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );   //将颜色图3通道 转换为灰度图单通道 8位无符号 对应边类的双线性插值计算放法以单通道计算的
214 
215         //第一帧为世界坐标系 计算FAST关键点 为之后与当前帧用直接法计算RT做准备
216         if ( index ==0 )//以第一帧为参考系 计算关键点后存储测量值(关键点对应的灰度值) 以此为基准跟踪后面的图像 计算位姿
217         {
218             // 对第一帧提取FAST特征点
219             vector<cv::KeyPoint> keypoints;
220             cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
221             detector->detect ( color, keypoints );
222             //对于2D关键点获取 3D信息 并去掉范围外的点 存储符合要求的关键点的深度值和3D信息
223             //对所有关键点挑选出符合要求且有深度值的 存储到vector<Measurement> measurements中 为g2o边提供灰度测量值和空间点坐标
224             for ( auto kp:keypoints )
225             {
226                 // 去掉邻近边缘处的点 在离图像四条边20个像素构成的内矩阵范围内是符合要求的关键点
227                 if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
228                     continue;
229                 //depth.ptr<ushort>( kp.pt.y)获取行指针  cvRound(kp.pt,y) 表示返回跟参数值最接近的整数值 因为像素量化后是整数,而kp.pt.y存储方式是float,所以强制转换一下即可
230                 ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//16位深度图
231                 if ( d==0 )
232                     continue;
233                 Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );       //3D相机坐标系(第一帧 也是世界帧)
234         //计算小块平均灰度值作为对应单一像素的测量值 增加算法健壮性
235                 float grayscale = getPatchAverageGray( gray , kp.pt.x , kp.pt.y);
236 
237 
238                 measurements.push_back ( Measurement ( p3d, grayscale ) );
239             }
240             prev_color = color.clone();     //深拷贝color图像
241             continue;
242         }
243         // 使用直接法计算相机运动
244         //从第二帧开始计算相机位姿g2o优化
245         chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
246         //优化过程中要提供灰度图像 边里面计算误差函数需要 为getPixelValue()该函数提供灰度值查找
247         poseEstimationDirect ( measurements, &gray, K, Tcw );//Tcw为世界坐标到下一帧坐标的累计值 最后Tcw的结果是从世界坐标 到当前帧下的转换
248         chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
249         chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
250         cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
251         cout<<"Tcw="<<Tcw.matrix() <<endl;
252 
253         // plot the feature points
254         cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是为了之后对比前后两帧图像的关键点数量 所以建立一个可以存储pre_color 和color 大小的矩阵
255         //Rect(参数)表示坐标0,0 到cols,rows 那么大的矩形
256         //img_show.operator(const Rect &roi 参数:表示这个矩阵的某个兴趣区域)
257         //img_show.opertor返回一个构造的矩阵 Mat(const Mat& m, const Rect& roi);这个构造函数返回引用m矩阵中roi那部分感兴趣的范围
258         //最终结果是:prev_color矩阵元素拷贝到了 img_show矩阵对应Rect兴趣区域 因为img_show 是一个2*row行 cols列 可以包含两个prev_color矩阵
259         prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列 0行 ->cols列 rows行 大小 //实际上就是把第一帧的图像拷贝到img_show中
260                                                                                     //因为我们针对每一帧图像都会把第一帧图像拷贝到这里 所以这里实际上执行一次即可
261                                                                                     //可以修改 前加上仅仅对第二帧执行一次即可
262         color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列 rows行 ->cols列 rows行 大小
263 
264         //在measurements容器中 随机挑选出符合要求的测量值 在img_show矩阵中对应部分进行标记(因为img_show上半部分是第一帧图像,下半部分是当前图像)
265         for ( Measurement m:measurements )
266         {
267             if ( rand() > RAND_MAX/5 )
268                 continue;
269             Eigen::Vector3d p = m.pos_world;
270             Eigen::Vector2d pixel_prev = project3Dto2D ( float( p ( 0,0 ) ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//世界坐标系下的 图像坐标2D
271             Eigen::Vector3d p2 = Tcw*m.pos_world;//将空间点转换到下一帧相机坐标系下
272             Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//当前帧坐标系下的图像像素坐标
273             //对于超出下一帧图像像素坐标轴范围的点 舍弃不画
274             if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
275                 continue;
276             //随机获取bgr颜色 在cv::circle中 为关键点用不同的颜色圆来画出
277             float b = 255*float ( rand() ) /RAND_MAX;
278             float g = 255*float ( rand() ) /RAND_MAX;
279             float r = 255*float ( rand() ) /RAND_MAX;
280             //在img_show包含两帧图像上 以关键点为圆心画圆 半径为8个像素 颜色为bgr随机组合  2表示外轮廓线宽度为2 如果为负数则表示填充圆
281             //pixel_prev 都是世界坐标系下的坐标 (以第一帧为参考系) 和当前帧下的对比 可以看出关键点的数量会逐渐减少
282             cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
283             cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );//注意这里+color.rows 当前帧在img_show的下半部分
284             //连接前后两针匹配好的点
285             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 ( 0,0,250 ), 1 );
286         }
287         cv::imshow ( "result", img_show );
288         cv::waitKey ( 0 );
289 
290     }
291     return 0;
292 }
293 
294 void poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
295 {
296     // 初始化g2o
297     typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6*1的
298     DirectBlock::LinearSolverType* linearSolver = new  g2o::LinearSolverDense< DirectBlock::PoseMatrixType >();
299     DirectBlock* solver_ptr = new DirectBlock(unique_ptr<DirectBlock::LinearSolverType>(linearSolver));
300     g2o::OptimizationAlgorithmLevenberg *solver =new g2o::OptimizationAlgorithmLevenberg(unique_ptr<DirectBlock>(solver_ptr));
301     // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
302 
303     g2o::SparseOptimizer optimizer;
304     optimizer.setAlgorithm ( solver );
305     optimizer.setVerbose( true );
306 
307     auto pose = new g2o::VertexSE3Expmap();
308     pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
309     pose->setId ( 0 );
310     optimizer.addVertex ( pose );
311 
312     // 添加边
313     int id=1;
314     for ( Measurement m: measurements )
315     {
316         auto edge = new EdgeSE3ProjectDirect (
317             m.pos_world,
318             K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
319         );
320         edge->setVertex ( 0, pose );//设置一元边链接的顶点
321         edge->setMeasurement ( m.grayscale );//设置测量值 即把前一帧关键点的灰度值作为测量值 供给下一帧进行匹配计算RT
322         edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//因为误差维度是1 所以信心矩阵为1x1
323         edge->setId ( id++ );
324         optimizer.addEdge ( edge );
325     }
326     cout<<"edges in graph: "<<optimizer.edges().size() <<endl;//边的个数 实际上反应了关键点的个数
327     optimizer.initializeOptimization();
328     optimizer.optimize ( 30 );
329     Tcw = pose->estimate();
330 }
331 
332 
333 //获取小块平均灰度值
334 // gray:灰度矩阵 x,y表示以(x,y)为中心  计算的小块的平均灰度
335 float getPatchAverageGray(const cv::Mat &gray ,float u, float v){
336     int x = cvRound(u);
337     int y = cvRound(v);
338    
339     float grayscale = 0.0;
340 
341     for (int j = y-1; j <= y+1 ; ++j)
342         for(auto i = x-1;i<= x+1; ++i){
343             grayscale += float ( gray.ptr<uchar> (j)[i] );
344         }
345     //grayscale/= ( (2*patchRadius + 1)*(2*patchRadius +1) );
346     grayscale/=9;
347     return grayscale;
348 }

 

4.使用Ceres实现RGB-D上的稀疏直接法和半稠密直接法。

 

 

 

 

posted @ 2018-12-09 17:05  Iridescent18  阅读(819)  评论(0编辑  收藏  举报