8 int num_points =
sba.tracks.size();
9 int num_cameras =
sba.nodes.size();
10 if (num_points == 0 && num_cameras == 0)
return;
12 visualization_msgs::Marker camera_marker, point_marker;
13 camera_marker.header.frame_id =
"/pgraph";
15 camera_marker.ns =
"pgraph";
17 camera_marker.action = visualization_msgs::Marker::ADD;
18 camera_marker.pose.position.x = 0;
19 camera_marker.pose.position.y = 0;
20 camera_marker.pose.position.z = 0;
21 camera_marker.pose.orientation.x = 0.0;
22 camera_marker.pose.orientation.y = 0.0;
23 camera_marker.pose.orientation.z = 0.0;
24 camera_marker.pose.orientation.w = 1.0;
25 camera_marker.scale.x = 0.02;
26 camera_marker.scale.y = 0.02;
27 camera_marker.scale.z = 0.02;
28 camera_marker.color.r = 0.0f;
29 camera_marker.color.g = 1.0f;
30 camera_marker.color.b = 1.0f;
31 camera_marker.color.a = 1.0f;
33 camera_marker.type = visualization_msgs::Marker::LINE_LIST;
35 point_marker = camera_marker;
36 point_marker.color.r = 1.0f;
37 point_marker.color.g = 0.0f;
38 point_marker.color.b = 0.0f;
39 point_marker.color.a = 0.5f;
40 point_marker.scale.x = 0.02;
41 point_marker.scale.y = 0.02;
42 point_marker.scale.z = 0.02;
43 point_marker.type = visualization_msgs::Marker::POINTS;
46 point_marker.points.resize((
int)(num_points/(
double)decimation + 0.5));
47 point_marker.colors.resize((
int)(num_points/(
double)decimation + 0.5));
48 for (
int i=0, ii=0; i < num_points; i += decimation, ii++)
50 const Vector4d &pt =
sba.tracks[i].point;
51 point_marker.colors[ii].r = 1.0f;
52 if (bicolor > 0 && i >= bicolor)
53 point_marker.colors[ii].g = 1.0f;
55 point_marker.colors[ii].g = 0.0f;
56 point_marker.colors[ii].b = 0.0f;
57 point_marker.points[ii].x = pt(2);
58 point_marker.points[ii].y = -pt(0);
59 point_marker.points[ii].z = -pt(1);
63 camera_marker.points.resize(num_cameras*6);
64 for (
int i=0, ii=0; i < num_cameras; i++)
68 Matrix<double,3,4> tr;
71 camera_marker.points[ii].x = nd.
trans.z();
72 camera_marker.points[ii].y = -nd.
trans.x();
73 camera_marker.points[ii++].z = -nd.
trans.y();
74 opt = tr*Vector4d(0,0,0.3,1);
75 camera_marker.points[ii].x = opt.z();
76 camera_marker.points[ii].y = -opt.x();
77 camera_marker.points[ii++].z = -opt.y();
79 camera_marker.points[ii].x = nd.
trans.z();
80 camera_marker.points[ii].y = -nd.
trans.x();
81 camera_marker.points[ii++].z = -nd.
trans.y();
82 opt = tr*Vector4d(0.2,0,0,1);
83 camera_marker.points[ii].x = opt.z();
84 camera_marker.points[ii].y = -opt.x();
85 camera_marker.points[ii++].z = -opt.y();
87 camera_marker.points[ii].x = nd.
trans.z();
88 camera_marker.points[ii].y = -nd.
trans.x();
89 camera_marker.points[ii++].z = -nd.
trans.y();
90 opt = tr*Vector4d(0,0.1,0,1);
91 camera_marker.points[ii].x = opt.z();
92 camera_marker.points[ii].y = -opt.x();
93 camera_marker.points[ii++].z = -opt.y();
97 int num_tracks =
sba.tracks.size();
98 int ii = camera_marker.points.size();
100 for (
int i=0; i < num_tracks; i++)
102 const ProjMap &prjs =
sba.tracks[i].projections;
103 for (ProjMap::const_iterator itr = prjs.begin(); itr != prjs.end(); itr++)
105 const Proj &prj = (*itr).second;
108 camera_marker.points.resize(ii+2);
112 Eigen::Vector3d w = pt0.head<3>()-plane_point;
114 Eigen::Vector3d projpt = pt0.head<3>() - (w.dot(plane_normal))*plane_normal;
116 Vector3d pt1 = projpt;
118 camera_marker.points[ii].x = pt0.z();
119 camera_marker.points[ii].y = -pt0.x();
120 camera_marker.points[ii++].z = -pt0.y();
121 camera_marker.points[ii].x = pt1.z();
122 camera_marker.points[ii].y = -pt1.x();
123 camera_marker.points[ii++].z = -pt1.y();
128 camera_pub.
publish(camera_marker);
129 point_pub.
publish(point_marker);