fixed vector size mismatch between matched and status vectors

loop_fusion/src/utility/CameraPoseVisualization.h

@@ -0,0 +1,58 @@
+/*******************************************************
+ * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
+ * 
+ * This file is part of VINS.
+ * 
+ * Licensed under the GNU General Public License v3.0;
+ * you may not use this file except in compliance with the License.
+ *******************************************************/
+
+#pragma once
+
+#include <ros/ros.h>
+#include <std_msgs/ColorRGBA.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <opencv2/opencv.hpp>
+#include "../parameters.h"
+
+class CameraPoseVisualization {
+public:
+	std::string m_marker_ns;
+
+	CameraPoseVisualization(float r, float g, float b, float a);
+	
+	void setImageBoundaryColor(float r, float g, float b, float a=1.0);
+	void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0);
+	void setScale(double s);
+	void setLineWidth(double width);
+
+	void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
+	void reset();
+
+	void publish_by(ros::Publisher& pub, const std_msgs::Header& header);
+	void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
+	void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
+	//void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src);
+	void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header);
+private:
+	std::vector<visualization_msgs::Marker> m_markers;
+	std_msgs::ColorRGBA m_image_boundary_color;
+	std_msgs::ColorRGBA m_optical_center_connector_color;
+	double m_scale;
+	double m_line_width;
+	visualization_msgs::Marker image;
+	int LOOP_EDGE_NUM;
+	int tmp_loop_edge_num;
+
+	static const Eigen::Vector3d imlt;
+	static const Eigen::Vector3d imlb;
+	static const Eigen::Vector3d imrt;
+	static const Eigen::Vector3d imrb;
+	static const Eigen::Vector3d oc  ;
+	static const Eigen::Vector3d lt0 ;
+	static const Eigen::Vector3d lt1 ;
+	static const Eigen::Vector3d lt2 ;
+};