class ImageConverter { ros::NodeHandle nh; imagetransport::ImageTransport it; imagetransport::Subscriber imagesub; imagetransport::Publisher imagepub; imagetransport::Publisher imagepub1; ros::Publisher pub = nh.advertise ("points2", 1); ros::Publisher markerarraypub = nh.advertise

("visualization_marker_array", 1);


       ros::Publisher marker_pub;
        visualization_msgs::MarkerArray marker_array, arrow_array, vally_array;
        visualization_msgs::Marker marker;

public:
    ImageConverter()
        : it_(nh_)
    {
        // Subscrive to input video feed and publish output video feed
        image_sub_ = it_.subscribe("/cv_camera/image_raw", 1,
                                   &ImageConverter::imageCb, this);
    image_pub_ = it_.advertise("/image_converter/output_video", 1);
    image_pub1_ = it_.advertise("/image_converter1/output_video", 1);

marker_pub = nh_.advertise<visualization_msgs::Marker> ("visualization_marker",1);
marker.header.frame_id = "/base_frame";
marker.header.stamp = ros::Time();
marker.ns = "points";
marker.action = visualization_msgs::Marker::ADD;
marker.pose.orientation.w = 1;
marker.id = 1;
marker.type = visualization_msgs::Marker::POINTS;
    marker.scale.x = 1.0f;
    marker.scale.y = 1.0f;
        marker.scale.z = 1.0f;
        marker.color.a = 1.0f;
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;

        marker.lifetime = ros::Duration();
    }

~ImageConverter()

{
}
//Eigen::Vector3d GetPoint(Eigen::Vector3d p0,double theta,const cv::Mat img);
void SetMarkerArray(visualization_msgs::Marker marker,visualization_msgs::MarkerArray &marker_array,Eigen::Vector3d pi,uint32_t type,std::size_t marker_id,std_msgs::ColorRGBA color,Eigen::Vector3d p0={-1,-1,-1});

void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }
            ...
                    else if (has_free_plane_below) {
                        p.x = p0.x(); p.y = p0.y(); p.z = p0.z();
                        pc.r = 1.0f; pc.g = 0.0f; pc.b = 0.0f; pc.a = 1.0f;
                        marker.points.push_back(p);
                        marker.colors.push_back(pc);
                        //std::cout<<"vally2"<<std::endl;
                        resImg.at<cv::Vec3b>(x0,y0) = {0,0,255};
                    }
    ...
            marker_pub.publish(marker);

Above is the small code and screenshot of the result of my algorithm. Here i am trying to visualize different points in rviz with different colors but unfortunatly i'm not able to solve this problem till now i.e not able to give color to points.