该方法详述了高博kinect2_viewer的编译过程
//.................................................................................
//單獨編譯kinect2_viewer,實現KInectv2 ORBSLAM2運行
//viewer.cpp修改參考“http://www.cnblogs.com/gaoxiang12/p/5161223.html”
//kinect2_viewer編譯參考“http://www.cnblogs.com/bigzhao/p/6278993.html”
1.修改viewer.cpp參考“http://www.cnblogs.com/gaoxiang12/p/5161223.html”
注意:
viewer.cpp中将原来的#include "orbslam2/System.h"改为现在的#include "System.h"
這兩行也對應修改
string orbVocFile = "/home/tommy/catkin_ws/src/tommy/config/ORBvoc.txt";
string orbSetiingsFile = "/home/tommy/catkin_ws/src/tommy/config/kinect2_qhd.yaml";
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <cmath>
#include <mutex>
#include <thread>
#include <chrono>
#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <kinect2_bridge/kinect2_definitions.h>
#include "System.h"
class Receiver
{
public:
enum Mode
{
IMAGE = 0,
CLOUD,
BOTH
};
private:
std::mutex lock;
const std::string topicColor, topicDepth;
const bool useExact, useCompressed;
bool updateImage, updateCloud;
bool save;
bool running;
size_t frame;
const size_t queueSize;
cv::Mat color, depth;
cv::Mat cameraMatrixColor, cameraMatrixDepth;
cv::Mat lookupX, lookupY;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactSyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximateSyncPolicy;
ros::NodeHandle nh;
ros::AsyncSpinner spinner;
image_transport::ImageTransport it;
image_transport::SubscriberFilter *subImageColor, *subImageDepth;
message_filters::Subscriber<sensor_msgs::CameraInfo> *subCameraInfoColor, *subCameraInfoDepth;
message_filters::Synchronizer<ExactSyncPolicy> *syncExact;
message_filters::Synchronizer<ApproximateSyncPolicy> *syncApproximate;
std::thread imageViewerThread;
Mode mode;
std::ostringstream oss;
std::vector<int> params;
//RGBDSLAM slam; //the slam object
ORB_SLAM2::System* orbslam =nullptr;
public:
Receiver(const std::string &topicColor, const std::string &topicDepth, const bool useExact, const bool useCompressed)
: topicColor(topicColor), topicDepth(topicDepth), useExact(useExact), useCompressed(useCompressed),
updateImage(false), updateCloud(false), save(false), running(false), frame(0), queueSize(5),
nh("~"), spinner(0), it(nh), mode(CLOUD)
{
cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F);
cameraMatrixDepth = cv::Mat::zeros(3, 3, CV_64F);
params.push_back(cv::IMWRITE_JPEG_QUALITY);
params.push_back(100);
params.push_back(cv::IMWRITE_PNG_COMPRESSION);
params.push_back(1);
params.push_back(cv::IMWRITE_PNG_STRATEGY);
params.push_back(cv::IMWRITE_PNG_STRATEGY_RLE);
params.push_back(0);
string orbVocFile = "/home/tommy/catkin_ws/src/tommy/config/ORBvoc.txt";
string orbSetiingsFile = "/home/tommy/catkin_ws/src/tommy/config/kinect2_qhd.yaml";
orbslam = new ORB_SLAM2::System( orbVocFile, orbSetiingsFile ,ORB_SLAM2::System::RGBD, true );
}
~Receiver()
{
if (orbslam)
{
orbslam->Shutdown();
delete orbslam;
}
}
void run(const Mode mode)
{
start(mode);
stop();
}
void finish()
{
}
private:
void start(const Mode mode)
{
this->mode = mode;
running = true;
std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";
image_transport::TransportHints hints(useCompressed ? "compressed" : "raw");
subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);
if(useExact)
{
syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
}
else
{
syncApproximate = new message_filters::Synchronizer<ApproximateSyncPolicy>(ApproximateSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
syncApproximate->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
}
spinner.start();
std::chrono::milliseconds duration(1);
while(!updateImage || !updateCloud)
{
if(!ros::ok())
{
return;
}
std::this_thread::sleep_for(duration);
}
createLookup(this->color.cols, this->color.rows);
switch(mode)
{
case IMAGE:
imageViewer();
break;
case BOTH:
imageViewerThread = std::thread(&Receiver::imageViewer, this);
break;
}
}
void stop()
{
spinner.stop();
if(useExact)
{
delete syncExact;
}
else
{
delete syncApproximate;
}
delete subImageColor;
delete subImageDepth;
delete subCameraInfoColor;
delete subCameraInfoDepth;
running = false;
if(mode == BOTH)
{
imageViewerThread.join();
}
}
void callback(const sensor_msgs::Image::ConstPtr imageColor, const sensor_msgs::Image::ConstPtr imageDepth,
const sensor_msgs::CameraInfo::ConstPtr cameraInfoColor, const sensor_msgs::CameraInfo::ConstPtr cameraInfoDepth)
{
cv::Mat color, depth;
readCameraInfo(cameraInfoColor, cameraMatrixColor);
readCameraInfo(cameraInfoDepth, cameraMatrixDepth);
readImage(imageColor, color);
readImage(imageDepth, depth);
// IR image input
if(color.type() == CV_16U)
{
cv::Mat tmp;
color.convertTo(tmp, CV_8U, 0.02);
cv::cvtColor(tmp, color, CV_GRAY2BGR);
}
lock.lock();
this->color = color;
this->depth = depth;
updateImage = true;
updateCloud = true;
lock.unlock();
}
void imageViewer()
{
cv::Mat color, depth;
for(; running && ros::ok();)
{
if(updateImage)
{
lock.lock();
color = this->color;
depth = this->depth;
updateImage = false;
lock.unlock();
if (orbslam)
{
orbslam->TrackRGBD( color, depth, ros::Time::now().toSec() );
}
}
}
cv::destroyAllWindows();
cv::waitKey(100);
}
void readImage(const sensor_msgs::Image::ConstPtr msgImage, cv::Mat &image) const
{
cv_bridge::CvImageConstPtr pCvImage;
pCvImage = cv_bridge::toCvShare(msgImage, msgImage->encoding);
pCvImage->image.copyTo(image);
}
void readCameraInfo(const sensor_msgs::CameraInfo::ConstPtr cameraInfo, cv::Mat &cameraMatrix) const
{
double *itC = cameraMatrix.ptr<double>(0, 0);
for(size_t i = 0; i < 9; ++i, ++itC)
{
*itC = cameraInfo->K[i];
}
}
void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue)
{
cv::Mat tmp = cv::Mat(in.rows, in.cols, CV_8U);
const uint32_t maxInt = 255;
#pragma omp parallel for
for(int r = 0; r < in.rows; ++r)
{
const uint16_t *itI = in.ptr<uint16_t>(r);
uint8_t *itO = tmp.ptr<uint8_t>(r);
for(int c = 0; c < in.cols; ++c, ++itI, ++itO)
{
*itO = (uint8_t)std::min((*itI * maxInt / maxValue), 255.0f);
}
}
cv::applyColorMap(tmp, out, cv::COLORMAP_JET);
}
void combine(const cv::Mat &inC, const cv::Mat &inD, cv::Mat &out)
{
out = cv::Mat(inC.rows, inC.cols, CV_8UC3);
#pragma omp parallel for
for(int r = 0; r < inC.rows; ++r)
{
const cv::Vec3b
*itC = inC.ptr<cv::Vec3b>(r),
*itD = inD.ptr<cv::Vec3b>(r);
cv::Vec3b *itO = out.ptr<cv::Vec3b>(r);
for(int c = 0; c < inC.cols; ++c, ++itC, ++itD, ++itO)
{
itO->val[0] = (itC->val[0] + itD->val[0]) >> 1;
itO->val[1] = (itC->val[1] + itD->val[1]) >> 1;
itO->val[2] = (itC->val[2] + itD->val[2]) >> 1;
}
}
}
void createLookup(size_t width, size_t height)
{
const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
const float cx = cameraMatrixColor.at<double>(0, 2);
const float cy = cameraMatrixColor.at<double>(1, 2);
float *it;
lookupY = cv::Mat(1, height, CV_32F);
it = lookupY.ptr<float>();
for(size_t r = 0; r < height; ++r, ++it)
{
*it = (r - cy) * fy;
}
lookupX = cv::Mat(1, width, CV_32F);
it = lookupX.ptr<float>();
for(size_t c = 0; c < width; ++c, ++it)
{
*it = (c - cx) * fx;
}
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "kinect2_slam", ros::init_options::AnonymousName);
if(!ros::ok())
{
return 0;
}
std::string topicColor = "/kinect2/sd/image_color_rect";
std::string topicDepth = "/kinect2/sd/image_depth_rect";
bool useExact = true;
bool useCompressed = false;
Receiver::Mode mode = Receiver::IMAGE;
// 初始化receiver
Receiver receiver(topicColor, topicDepth, useExact, useCompressed);
//OUT_INFO("starting receiver...");
receiver.run(mode);
receiver.finish();
ros::shutdown();
return 0;
}
ORBvoc.txt和kinect2_qhd.yaml文件拷貝到和上面對應的文件夾中
2.拷貝ORBSLAM2頭文件到kinect2_viewer文件夾,所有.h文件包含在include文件夾裏面,include文件夾裏面加入orbslam2的Thirdparty文件夾,include文件夾裏面加入libORB_SLAM2.so,按照如下方式修改cmakelist文件
//cmakelist中修改
find_package(Pangolin REQUIRED)
find_package(Eigen3 3.1.0 REQUIRED)
include_directories(include
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${kinect2_bridge_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
)
add_executable(kinect2_viewer src/viewer.cpp)
target_link_libraries(kinect2_viewer
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
${kinect2_bridge_LIBRARIES}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/include/libORB_SLAM2.so
${PROJECT_SOURCE_DIR}/include/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/include/Thirdparty/g2o/lib/libg2o.so
)
3. 編譯
a).cd /home/tommy/catkin_ws/src/iai_kinect2/kinect2_viewer/build
b).cmake ..
c).make
注意:出现/ws/ORB_SLAM2/src/LoopClosing.cc: In member function ‘void ORB_SLAM2::LoopClosing::Run()’:
/ws/ORB_SLAM2/src/LoopClosing.cc:84:20: error: ‘usleep’ was not declared in this scope
usleep(5000);
需要打开相应的代码,在头文件里面添加usleep 的头文件unistd.h,问题就解决了!
參考“http://blog.csdn.net/wangshuailpp/article/details/70226534”
//.................................................................................