單目視覺裡程計的尺度問題的疑問？

SLAM是個坑…

經過一段時間對SLAM的了解，原來是我了解有錯誤，得到的尺度已經是統一的，隻是不知道它的真實尺度是多少。

貼一個不錯的視覺裡程計簡介的連結：

https://blog.csdn.net/gzj2013/article/details/100878053

關于下面代碼中單目視覺裡程計中尺度的了解：

https://github.com/yueying/LearningVO/blob/master/src/visual_odometry.cpp

截取片段：

double VisualOdometry::getAbsoluteScale(int frame_id)
{
	std::string line;
	int i = 0;
	std::ifstream ground_truth("C:/dataset/00/00.txt");
	double x = 0, y = 0, z = 0;
	double x_prev, y_prev, z_prev;
	// 擷取目前幀真實位置與前一幀的真實位置的距離作為尺度值
	if (ground_truth.is_open())
	{
		while ((std::getline(ground_truth, line)) && (i <= frame_id))
		{
			z_prev = z;
			x_prev = x;
			y_prev = y;
			std::istringstream in(line);
			for (int j = 0; j < 12; j++)  {
				in >> z;
				if (j == 7) y = z;
				if (j == 3)  x = z;
			}
			i++;
		}
		ground_truth.close();
	}

	else {
		std::cerr<< "Unable to open file";
		return 0;
	}

	return sqrt((x - x_prev)*(x - x_prev) + (y - y_prev)*(y - y_prev) + (z - z_prev)*(z - z_prev));
}
           

從以上代碼來看，資料集KITTI-00的真實值pose.txt，裡面每行是12列資料，12列資料很容易想到是3個平移量和一個3x3的旋轉矩陣，這樣想沒錯，但是其排列方式卻不是這樣的，而是一個3*4的矩陣,其排列方式為一個增廣矩陣：[R|t]

也就是說，每一行的最後一列資料為平移的t的資料。

那麼是如何得到尺度因子的呢？衆所周知，單目無法得到真實尺度的資訊，不具有機關的概念，是以上面的基于單目的視覺裡程計其尺度資訊是來自于groundtruth的，也就是事先知道的真實尺度，如何計算：擷取目前幀真實位置與前一幀的真實位置的距離作為尺度值。

也就是最後return的值，其實就是目前幀的（x,y,z）減去上一幀的（x,y,z）這個真實距離作為真實尺度。

隻是我不明白為什麼更新尺度的時候是這樣計算的呢？

bool VisualOdometry::processFrame(int frame_id)
{
	double scale = 1.00;//初始尺度為1
	featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通過光流跟蹤确定第二幀中的相關特征
	cv::Mat E, R, t, mask;
	E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
	cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);
	scale = getAbsoluteScale(frame_id);//得到目前幀的實際尺度
	if (scale > 0.1) //如果尺度小于0.1可能計算出的Rt存在一定的問題,則不做處理，保留上一幀的值
	{
		cur_t_ = cur_t_ + scale*(cur_R_*t);
		cur_R_ = R*cur_R_;
	}
	// 如果跟蹤特征點數小于給定門檻值，進行重新特征檢測
	if (px_ref_.size() < kMinNumFeature)
	{
		featureDetection(new_frame_, px_ref_);
		featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_);
	}
	px_ref_ = px_cur_;
	return true;
}
           

其中第11行和第12行的代碼我不是很懂，得到了實際尺度，為什麼要那樣更新呢？我本以為使用如下這樣的更新方式就行了：

// 錯誤示範
if (scale > 0.1) 
{
		cur_t_ = cur_t_ + scale*t;
		cur_R_ = R*cur_R_;
}
/*之是以錯誤的原因是，忽略了平移的方向性，是以左乘旋轉矩陣，就規定了它朝哪個方向旋轉，這也符合真實的平移情況*/
           

我的了解是：先旋轉後平移再加上原來的平移量才是真實的平移，如上注釋。