本文整理汇总了C++中Estimator::getPoseInWorldFrame方法的典型用法代码示例。如果您正苦于以下问题：C++ Estimator::getPoseInWorldFrame方法的具体用法？C++ Estimator::getPoseInWorldFrame怎么用？C++ Estimator::getPoseInWorldFrame使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Estimator的用法示例。
在下文中一共展示了Estimator::getPoseInWorldFrame方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
int main(int argc, char** argv)
{
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
ros::Publisher pubLeftImage = n.advertise<sensor_msgs::Image>("/leftImage",1000);
ros::Publisher pubRightImage = n.advertise<sensor_msgs::Image>("/rightImage",1000);
if(argc != 3)
{
printf("please intput: rosrun vins kitti_odom_test [config file] [data folder] \n"
"for example: rosrun vins kitti_odom_test "
"~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml "
"/media/tony-ws1/disk_D/kitti/odometry/sequences/00/ \n");
return 1;
}
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
string sequence = argv[2];
printf("read sequence: %s\n", argv[2]);
string dataPath = sequence + "/";
readParameters(config_file);
estimator.setParameter();
registerPub(n);
// load image list
FILE* file;
file = std::fopen((dataPath + "times.txt").c_str() , "r");
if(file == NULL){
printf("cannot find file: %stimes.txt\n", dataPath.c_str());
ROS_BREAK();
return 0;
}
double imageTime;
vector<double> imageTimeList;
while ( fscanf(file, "%lf", &imageTime) != EOF)
{
imageTimeList.push_back(imageTime);
}
std::fclose(file);
string leftImagePath, rightImagePath;
cv::Mat imLeft, imRight;
FILE* outFile;
outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
if(outFile == NULL)
printf("Output path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());
for (size_t i = 0; i < imageTimeList.size(); i++)
{
if(ros::ok())
{
printf("\nprocess image %d\n", (int)i);
stringstream ss;
ss << setfill('0') << setw(6) << i;
leftImagePath = dataPath + "image_0/" + ss.str() + ".png";
rightImagePath = dataPath + "image_1/" + ss.str() + ".png";
//printf("%lu %f \n", i, imageTimeList[i]);
//printf("%s\n", leftImagePath.c_str() );
//printf("%s\n", rightImagePath.c_str() );
imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
sensor_msgs::ImagePtr imLeftMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
imLeftMsg->header.stamp = ros::Time(imageTimeList[i]);
pubLeftImage.publish(imLeftMsg);
imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );
sensor_msgs::ImagePtr imRightMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();
imRightMsg->header.stamp = ros::Time(imageTimeList[i]);
pubRightImage.publish(imRightMsg);
estimator.inputImage(imageTimeList[i], imLeft, imRight);
Eigen::Matrix<double, 4, 4> pose;
estimator.getPoseInWorldFrame(pose);
if(outFile != NULL)
fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
pose(1,0), pose(1,1), pose(1,2),pose(1,3),
pose(2,0), pose(2,1), pose(2,2),pose(2,3));
//cv::imshow("leftImage", imLeft);
//cv::imshow("rightImage", imRight);
//cv::waitKey(2);
}
else
break;
}
if(outFile != NULL)
fclose (outFile);
return 0;
}