rviz学习

今天学习rviz，实验了贺一家博士的《从0开始手写vio》课件中的生成模拟IMU数据这一章节，代码如下：

/*
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

// %Tag(FULLTEXT)%
// %Tag(INCLUDES)%
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseStamped.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <fstream>
#include <sys/stat.h>
#include "imu.h"
#include "utilities.h"
// %EndTag(INCLUDES)%

using Point = Eigen::Vector4d;
using Points = std::vector<Point, Eigen::aligned_allocator<Point> >;
using Line = std::pair<Eigen::Vector4d, Eigen::Vector4d>;
using Lines = std::vector<Line, Eigen::aligned_allocator<Line> >;

void CreateHousePointsLines(Points& points, Lines& lines)
{
    std::ifstream f;
    f.open("/home/xueyufei/catkin_ws/src/vio_simulator/house_model/house.txt");

    while(!f.eof())
    {
        std::string s;
        std::getline(f,s);
        if(!s.empty())
        {
            std::stringstream ss;
            ss << s;
            double x,y,z;
            ss >> x;
            ss >> y;
            ss >> z;
            Eigen::Vector4d pt0( x, y, z, 1 );
            ss >> x;
            ss >> y;
            ss >> z;
            Eigen::Vector4d pt1( x, y, z, 1 );

            bool isHistoryPoint = false;
            for (int i = 0; i < points.size(); ++i) {
                Eigen::Vector4d pt = points[i];
                if(pt == pt0)
                {
                    isHistoryPoint = true;
                }
            }
            if(!isHistoryPoint)
                points.push_back(pt0);

            isHistoryPoint = false;
            for (int i = 0; i < points.size(); ++i) {
                Eigen::Vector4d pt = points[i];
                if(pt == pt1)
                {
                    isHistoryPoint = true;
                }
            }
            if(!isHistoryPoint)
                points.push_back(pt1);

            lines.emplace_back(pt0, pt1);   // lines
        }
    }

    // create more 3d points, you can comment this code
    int n = points.size();
    for (int j = 0; j < n; ++j) {
        Eigen::Vector4d p = points[j] + Eigen::Vector4d(0.5, 0.5, -0.5, 0);
        points.push_back(p);
    }
}

visualization_msgs::Marker CreateHousePointMarker(const Points& house_points)
{
    visualization_msgs::Marker marker_points;
    marker_points.header.frame_id = "/my_frame";
    marker_points.header.stamp = ros::Time::now();
    marker_points.ns = "house";
    marker_points.id = 0;
    marker_points.type = visualization_msgs::Marker::POINTS;
    marker_points.action = visualization_msgs::Marker::ADD;
    marker_points.pose.orientation.w =  1.0;

    marker_points.scale.x = 0.1;
    marker_points.scale.y = 0.1;

    marker_points.color.r = 0.0f;
    marker_points.color.g = 1.0f;
    marker_points.color.b = 0.0f;
    marker_points.color.a = 1.0;
    marker_points.lifetime = ros::Duration();

    for (uint32_t i = 0; i < house_points.size(); ++i)
    {
        geometry_msgs::Point p;
        p.x = house_points[i].x();
        p.y = house_points[i].y();
        p.z = house_points[i].z();
        marker_points.points.push_back(p);
    }
    return marker_points;
}

visualization_msgs::Marker CreateHouseLinesMarker(const Lines& house_lines)
{
    visualization_msgs::Marker marker_lines;
    marker_lines.header.frame_id = "/my_frame";
    marker_lines.header.stamp = ros::Time::now();
    marker_lines.ns = "house";
    marker_lines.id = 1;
    marker_lines.type = visualization_msgs::Marker::LINE_LIST;
    marker_lines.action = visualization_msgs::Marker::ADD;
    marker_lines.pose.orientation.w = 1.0;

    marker_lines.scale.x = 0.1;
    marker_lines.scale.y = 0.1;

    marker_lines.color.r = 1.0f;
    marker_lines.color.g = 1.0f;
    marker_lines.color.b = 1.0f;
    marker_lines.color.a = 1.0;
    marker_lines.lifetime = ros::Duration();

    for (uint32_t i = 0; i < house_lines.size(); ++i)
    {
        geometry_msgs::Point p0;
        p0.x = house_lines[i].first.x();
        p0.y = house_lines[i].first.y();
        p0.z = house_lines[i].first.z();
        marker_lines.points.push_back(p0);

        geometry_msgs::Point p1;
        p1.x = house_lines[i].second.x();
        p1.y = house_lines[i].second.y();
        p1.z = house_lines[i].second.z();
        marker_lines.points.push_back(p1);
    }
    return marker_lines;
}

visualization_msgs::Marker CreateTrajMarker()
{
    visualization_msgs::Marker traj_marker;
    traj_marker.header.frame_id = "/my_frame";
    traj_marker.header.stamp = ros::Time::now();
    traj_marker.ns = "house";
    traj_marker.id = 2;
    traj_marker.type =  visualization_msgs::Marker::LINE_STRIP;
    traj_marker.action =  visualization_msgs::Marker::ADD;
    traj_marker.pose.orientation.w = 1.0;

    traj_marker.scale.x = 0.1;
    traj_marker.scale.y = 0.1;

    traj_marker.color.r = 1.0f;
    traj_marker.color.g = 0.0f;
    traj_marker.color.b = 0.0f;
    traj_marker.color.a = 1.0;
    traj_marker.lifetime = ros::Duration();
    return traj_marker;
}

MotionData GenerateImuData(IMU& imuGen, float t)
{
    MotionData data = imuGen.MotionModel(t);
    MotionData data_noise = data;
    imuGen.addIMUnoise(data_noise);
    return data_noise;
}

Eigen::Vector2d p2c(const Eigen::Vector3d& pos, Param param)
{
    Eigen::Matrix3d cam;
    cam << param.fx, 0, param.cx,
        0, param.fy, param.cy, 
        0, 0, 1;
    Eigen::Vector3d uvz = cam * pos;
    Eigen::Vector2d uv;
    uv.x() = uvz.x() / uvz.z();
    uv.y() = uvz.y() / uvz.z();
    return uv;
}

std::vector<Eigen::Vector2d> GenerateImageFeatures(MotionData& imu_data, Param params, Points house_points)
{
    MotionData cam;
    cam.timestamp = imu_data.timestamp;
    cam.Rwb = imu_data.Rwb * params.R_bc;
    cam.twb = imu_data.twb + imu_data.Rwb * params.t_bc; //  Tcw = Twb * Tbc ,  t = Rwb * tbc + twb
    Eigen::Matrix4d Twc = Eigen::Matrix4d::Identity();
    Twc.block(0, 0, 3, 3) = cam.Rwb;
    Twc.block(0, 3, 3, 1) = cam.twb;
    std::vector<Eigen::Vector2d> features_cam;
    for (int i = 0; i < house_points.size(); ++i)
    {
        Eigen::Vector4d pw = house_points[i]; 
        Eigen::Vector4d pc1 = Twc.inverse() * pw; // T_wc.inverse() * Pw  -- > point in cam frame
        if (pc1(2) < 0)
            continue;

        Eigen::Vector2d uv = p2c(pc1.topLeftCorner(3, 1), params);
        features_cam.push_back(uv);
    }
    return features_cam;
}

std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> GenerateImageHouse(MotionData& imu_data, Param params, Lines house_lines)
{
    MotionData cam;
    cam.timestamp = imu_data.timestamp;
    cam.Rwb = imu_data.Rwb * params.R_bc;
    cam.twb = imu_data.twb + imu_data.Rwb * params.t_bc; //  Tcw = Twb * Tbc ,  t = Rwb * tbc + twb
    Eigen::Matrix4d Twc = Eigen::Matrix4d::Identity();
    Twc.block(0, 0, 3, 3) = cam.Rwb;
    Twc.block(0, 3, 3, 1) = cam.twb;
    std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> lines_cam; 
    for (int i = 0; i < house_lines.size(); ++i)
    {
        Line line = house_lines[i]; 
        Eigen::Vector4d pc0 = Twc.inverse() * line.first; // T_wc.inverse() * Pw  -- > point in cam frame
        Eigen::Vector4d pc1 = Twc.inverse() * line.second; // T_wc.inverse() * Pw  -- > point in cam frame
        if (pc0(2) < 0 && pc1(2) < 0)
            continue;

        Eigen::Vector2d uv0 = p2c(pc0.topLeftCorner(3, 1), params);
        Eigen::Vector2d uv1 = p2c(pc1.topLeftCorner(3, 1), params);
        lines_cam.emplace_back(uv0, uv1);
    }
    return lines_cam;
}

int main( int argc, char** argv )
{
  ros::init(argc, argv, "vio_simulator_xyf");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Publisher pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose_marker", 10);
  
  image_transport::ImageTransport it(n);
  image_transport::Publisher cam_pub = it.advertise("camera/image", 1);
  ros::Rate r(30);

  Points house_points;
  Lines house_lines;
  CreateHousePointsLines(house_points, house_lines);

  visualization_msgs::Marker traj_marker = CreateTrajMarker();
  visualization_msgs::Marker marker_points = CreateHousePointMarker(house_points);
  visualization_msgs::Marker marker_lines = CreateHouseLinesMarker(house_lines);

  Param params;
  IMU imuGen(params);
  float t = params.t_start;

  while (ros::ok())
  {
    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    ROS_WARN_ONCE("succeed!");
    marker_pub.publish(marker_points);
    marker_pub.publish(marker_lines);

    MotionData imu_data = GenerateImuData(imuGen, t);
    Eigen::Quaterniond q(imu_data.Rwb);

    geometry_msgs::PoseStamped po;
    po.pose.position.x = imu_data.twb.x();
    po.pose.position.y = imu_data.twb.y();
    po.pose.position.z = imu_data.twb.z();
    po.pose.orientation.x = q.x();
    po.pose.orientation.y = q.y();
    po.pose.orientation.z = q.z();
    po.pose.orientation.w = q.w();
    po.header.frame_id= "/my_frame";
    pose_pub.publish(po);

    geometry_msgs::Point traj_point;
    traj_point.x = imu_data.twb.x();
    traj_point.y = imu_data.twb.y();
    traj_point.z = imu_data.twb.z();
    traj_marker.points.push_back(traj_point);
    marker_pub.publish(traj_marker);

    std::vector<Eigen::Vector2d>  features_cam = GenerateImageFeatures(imu_data, params, house_points);
    std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> line2ds = GenerateImageHouse(imu_data, params, house_lines);
    cv::Mat image(params.image_h, params.image_w,  CV_8UC3, cv::Scalar(0, 0, 0));
    for(Eigen::Vector2d feature : features_cam) 
    {
        cv::circle(image, cv::Point(feature.x(), feature.y()), 1.0, cv::Scalar(0, 255, 255), 2);
    }
    for(auto line2d : line2ds) 
    {
        cv::line(image, cv::Point(line2d.first.x(), line2d.first.y()), cv::Point(line2d.second.x(), line2d.second.y()), cv::Scalar(255, 255, 255), 2);
    }
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    cam_pub.publish(msg);

    t += 1.0 / params.imu_frequency;
    r.sleep();
  }
}

效果如下