visualization.cpp
Go to the documentation of this file.
2 
3 namespace sba {
4 
5 void drawGraph(const SysSBA &sba, const ros::Publisher &camera_pub,
6  const ros::Publisher &point_pub, int decimation, int bicolor)
7 {
8  int num_points = sba.tracks.size();
9  int num_cameras = sba.nodes.size();
10  if (num_points == 0 && num_cameras == 0) return;
11 
12  visualization_msgs::Marker camera_marker, point_marker;
13  camera_marker.header.frame_id = "/pgraph";
14  camera_marker.header.stamp = ros::Time::now();
15  camera_marker.ns = "pgraph";
16  camera_marker.id = 0;
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;
32  camera_marker.lifetime = ros::Duration();
33  camera_marker.type = visualization_msgs::Marker::LINE_LIST;
34 
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;
44 
45  // draw points, decimated
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++)
49  {
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;
54  else
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);
60  }
61 
62  // draw cameras
63  camera_marker.points.resize(num_cameras*6);
64  for (int i=0, ii=0; i < num_cameras; i++)
65  {
66  const Node &nd = sba.nodes[i];
67  Vector3d opt;
68  Matrix<double,3,4> tr;
69  transformF2W(tr,nd.trans,nd.qrot);
70 
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();
78 
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();
86 
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();
94  }
95 
96  // draw point-plane projections
97  int num_tracks = sba.tracks.size();
98  int ii = camera_marker.points.size();
99 
100  for (int i=0; i < num_tracks; i++)
101  {
102  const ProjMap &prjs = sba.tracks[i].projections;
103  for (ProjMap::const_iterator itr = prjs.begin(); itr != prjs.end(); itr++)
104  {
105  const Proj &prj = (*itr).second;
106  if (prj.pointPlane) // have a ptp projection
107  {
108  camera_marker.points.resize(ii+2);
109  Point pt0 = sba.tracks[i].point;
110  Vector3d plane_point = prj.plane_point;
111  Vector3d plane_normal = prj.plane_normal;
112  Eigen::Vector3d w = pt0.head<3>()-plane_point;
113  // Eigen::Vector3d projpt = plane_point+(w.dot(plane_normal))*plane_normal;
114  Eigen::Vector3d projpt = pt0.head<3>() - (w.dot(plane_normal))*plane_normal;
115  // Vector3d pt1 = pt0.head<3>()+0.1*plane_normal;
116  Vector3d pt1 = projpt;
117 
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();
124  }
125  }
126  }
127 
128  camera_pub.publish(camera_marker);
129  point_pub.publish(point_marker);
130 }
131 
132 } // namespace sba
sba::Proj::plane_normal
Eigen::Vector3d plane_normal
Normal for point-plane projections.
Definition: proj.h:143
ros::Publisher
sba::SysSBA
SysSBA holds a set of nodes and points for sparse bundle adjustment.
Definition: sba.h:75
sba::Point
Eigen::Vector4d Point
Point holds 3D points using in world coordinates. Currently we just represent these as 4-vectors,...
Definition: node.h:43
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
sba
Definition: bpcg.h:60
sba::Node::qrot
Eigen::Quaternion< double > qrot
Rotation of the node expressed as a Quaternion.
Definition: node.h:70
sba::Proj::plane_point
Eigen::Vector3d plane_point
Point for point-plane projections.
Definition: proj.h:146
sba::ProjMap
std::map< const int, Proj, std::less< int >, Eigen::aligned_allocator< Proj > > ProjMap
Definition: proj.h:48
sba::Proj::pointPlane
bool pointPlane
Whether this is a point-plane match (true) or a point-point match (false).
Definition: proj.h:140
sba::Node::trans
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 4, 1 > trans
Translation in homogeneous coordinates, last element is 1.0.
Definition: node.h:69
sba::transformF2W
void transformF2W(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:166
sba::Node
NODE holds graph nodes corresponding to frames, for use in sparse bundle adjustment....
Definition: node.h:63
sba::Proj
Proj holds a projection measurement of a point onto a frame. They are a repository for the link betwe...
Definition: proj.h:57
ros::Duration
visualization.h
sba::drawGraph
void drawGraph(const SysSBA &sba, const ros::Publisher &camera_pub, const ros::Publisher &point_pub, int decimation=1, int bicolor=0)
Definition: visualization.cpp:5
ros::Time::now
static Time now()


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04