这篇博客主要介绍自己写的一个rgbd的前端里程计，并用TUM的数据集（包括ROS和非ROS的接口）进行测试。代码在这里
原理
前端里程计主要借助opencv的pnp算法实现，具体的思路是对每一帧提取特征，然后根据深度图计算出这些特征对应的三维点。对新来的图像做tracking，找到对应的特征，然后求解pnp就可以计算出位姿。对所有相邻的帧之间都进行这样的计算，就得到了视觉里程计。。果然还是很简单粗暴的实现啊，没有建图，没有优化，作为一个初始框架，以后有机会再往里添东西。
代码部分
除去主文件还剩视觉里程计和数据接口两个代码，先看接口部分的代码。
数据接口
接口部分完成了对TUM数据（ROS格式和非ROS格式）的读取。需要注意两种格式的深度数据不一样，ROS是直接的深度值，32位的float，无效点是nan
float d = depth_.ptr<float>(m)[n];
而非ROS的是16位的ushort，无效点是0
ushort d = depth_.ptr<ushort>(m)[n];
非ROS
在使用TUM非ROS数据前，首先要进行时间戳对齐，使用TUM提供的associate.py文件，这个可以直接在官网下载到，然后在数据集的路径下依次执行：
python associate.py rgb.txt depth.txt > associate.txt
python associate.py associate.txt groundtruth.txt > allData.txt
最后得到时间对齐后的所有数据，包括groundtruth。groundtruth主要是用来读取第一帧的世界坐标，因为最后用evo与groundtruth比较的时候，需要结果同样是groundtruth的世界坐标系，而不是我们计算的相对第一帧的世界坐标系。
读allData.txt的代码如下，比较简单，就不做解释了。。。
void readAssocTextfile(std::string filename,
std::vector<std::string>& inputRGBPaths,
std::vector<std::string>& inputDepthPaths,
std::vector<std::string>& inputTimeStamps,
std::vector<float>& pose) {
std::string line;
std::ifstream in_stream(filename.c_str());
bool readPose(false);
while (!in_stream.eof()) {
std::getline(in_stream, line);
std::stringstream ss(line);
std::string buf;
int count = 0;
while (ss >> buf) {
count++;
if (count == 2) {
inputRGBPaths.push_back(buf);
} else if (count == 4) {
inputDepthPaths.push_back(buf);
} else if (count == 1) {
inputTimeStamps.push_back(buf);
}
if(count == 4){
//get the first frame pose in the world
if(readPose)
break;
else{
ss >> buf;
for(int i=0; i<7; i++){
ss >> buf;
pose.push_back(atof(buf.c_str()));
std::cout<<pose[i]<<" ";
}
std::cout<<std::endl;
readPose = true;
}
}
}
}
in_stream.close();
}
ROS
ROS版本的数据读取就是topic的方式了，因为topic就是按照时间戳记录的，所以不需要做对齐，在回调函数里将图像传给VO，这部分在调试的过程中，比较担心因为计算VO比较费时而导致丢数据，但是因为这个实现简单粗暴，并不很费时间，所以可以实际测试并没有丢。
void dataPrepare::colorCallback(const sensor_msgs::ImageConstPtr& msg)
{
//ROS_INFO("color callback open");
//cout<<msg->header.stamp<<endl;
double h_time = msg->header.stamp.toSec();
try
{
color_ = cv_bridge::toCvShare(msg, "bgr8")->image;
vo_.setColor(color_);
// color msg comes later
if(!vo_.isInitialized()){
vo_.init();
}
else{
cv::Mat pose = vo_.estimatePose(h_time);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
//ROS_INFO("color callback end");
}
void dataPrepare::depthCallback(const sensor_msgs::ImageConstPtr& msg)
{
//ROS_INFO("depth callback open");
//cout<<msg->header.stamp<<endl;
try
{
depth_ = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_32FC1)->image;
vo_.setDepth(depth_);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'mono16'.", msg->encoding.c_str());
}
//ROS_INFO("callback end");
}
还有一部分是完成了发布odom以及发布点云，可以通过RVIZ来显示结果。
VO
VO的实现也是针对ROS与非ROS编写会有所不同，但内容相差不大，这里就只放一种
void VO::playSequence(const std::vector<std::string>& inputRGBPaths,
const std::vector<std::string>& inputDepthPaths,
const std::vector<std::string>& inputTimeStamps) {
cv::Mat color = cv::imread(inputRGBPaths[0]);
cv::Mat depth = cv::imread(inputDepthPaths[0], -1);
cv::Mat& ref_img = color;
vector<cv::Point3f> mapPoints;
vector<cv::Point2f> featurePoints;
extractKeyPoints(color, depth, mapPoints, featurePoints);
cv::Mat curImg;
cv::Mat traj = cv::Mat::zeros(600, 600, CV_8UC3);
std::string fname = "voPoses.txt";
std::ofstream f;
f.open(fname.c_str(), std::fstream::out);
for(int i=1; i<inputRGBPaths.size(); i++){
curImg = cv::imread(inputRGBPaths[i]);
cv::imshow( "image", curImg);
std::vector<cv::Point3f> mapPointsRef;
std::vector<cv::Point2f> featurePointsRef;
tracking(ref_img, curImg, featurePoints, mapPoints, mapPointsRef, featurePointsRef);
if(mapPointsRef.size() == 0)
continue;
cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F);
cv::Mat rvec, tvec;
vector<int> inliers;
//3d frame to 2d frame
cv::solvePnPRansac(mapPointsRef, featurePointsRef, K_, dist_coeffs,rvec, tvec, false, 100, 8.0, 0.99, inliers);
if(inliers.size() < 5)
continue;
float inliers_ratio = inliers.size()/float(mapPointsRef.size());
//cout << "inliers ratio: " << inliers_ratio << endl;
cv::Mat R_matrix;
cv::Rodrigues(rvec,R_matrix);
R_matrix = R_matrix.t();
cv::Mat t_vec = -R_matrix*tvec;
cv::Mat inv_transform = cv::Mat::eye(4,4,CV_64F);
R_matrix.copyTo(inv_transform.rowRange(0,3).colRange(0,3));
t_vec.copyTo(inv_transform.rowRange(0,3).col(3));
cv::Mat depth = cv::imread(inputDepthPaths[i], -1);
createNewPoints(curImg, depth, inv_transform, featurePoints, mapPoints);
//save pose.txt
std::stringstream strs;
strs << std::setprecision(6) << std::fixed << inputTimeStamps[i] << " ";
cv::Mat Twc = Two_ * inv_transform;
Eigen::Vector3d trans = toVector3d(Twc.rowRange(0,3).col(3));
Eigen::Matrix3d rot = toMatrix3d(Twc.rowRange(0,3).colRange(0,3));
f << strs.str() << trans(0) << " " << trans(1) << " " << trans(2) << " ";
Eigen::Quaterniond currentCameraRotation(rot);
f << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";
ref_img = curImg;
// plot the information
string text = "Red color: estimated trajectory";
t_vec.convertTo(t_vec, CV_32F);
//cout << t_vec.t() << endl;
//cout<<t_vec.at<float>(0)<<" "<<t_vec.at<float>(1)<<" "<<t_vec.at<float>(2)<<endl;
cv::Point2f center = cv::Point2f(int(60*t_vec.at<float>(0)) + 300, int(60*t_vec.at<float>(2)) + 100);
cv::circle(traj, center ,1, cv::Scalar(0,0,255), 2);
cv::rectangle(traj, cv::Point2f(10, 30), cv::Point2f(550, 50), cv::Scalar(0,0,0), cv::FILLED);
putText(traj, text, cv::Point2f(10,50), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0,255), 1, 5);
cv::imshow( "Trajectory", traj);
cv::waitKey(1);
}
f.close();
}
里面有几个关键函数：
- extractKeyPoints 提取当前帧的二维特征以及计算对应的三维点；
- tracking 计算相邻两帧之间的三维点与关键点的匹配；
- solvePnPRansac 计算pnp求解位姿；
- createNewPoints 计算下一帧的二维特征以及计算对应的三维点；
整个实现逻辑也比较简单，后面那部分矩阵的计算是转到groundtruth对应的坐标系下，然后就可以进行对比了。
测试
用TUM最最开始的数据集rgbd_dataset_freiburg1_xyz进行测试，结果emmmm当然很感人了。。。希望后续加优化之后，结果能有很大的提升吧
用evo工具得到的结果：
voPoses是自家的算法，rgbdslam是别人家的优秀算法
分析
当然不能满足于现在这个简单粗暴的版本，在此基础上还有很多改进的地方，最紧要的是先把后端优化以及建图加进去，以及考虑和其他传感器进行融合，这里就先埋下很多坑吧。。。
697
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
