Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "types_seven_dof_expmap.h"
00018
00019 #include "g2o/core/factory.h"
00020
00021 namespace g2o {
00022
00023 void __attribute__ ((constructor)) init_seven_dof_types(void)
00024 {
00025
00026 Factory* factory = Factory::instance();
00027 factory->registerType("VERTEX_SIM3:EXPMAP", new HyperGraphElementCreator<VertexSim3Expmap>);
00028
00029 factory->registerType("EDGE_SIM3:EXPMAP", new HyperGraphElementCreator<EdgeSim3>);
00030 factory->registerType("EDGE_PROJECT_SIM3_XYZ:EXPMAP", new HyperGraphElementCreator<EdgeSim3ProjectXYZ>);
00031 }
00032
00033 VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>()
00034 {
00035 _marginalized=false;
00036 _fix_scale = false;
00037 _principle_point[0] = 0;
00038 _principle_point[1] = 0;
00039 _focal_length[0] = 1;
00040 _focal_length[1] = 1;
00041 }
00042
00043
00044 EdgeSim3::EdgeSim3() :
00045 BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>()
00046 {
00047 }
00048
00049
00050 bool VertexSim3Expmap::read(std::istream& is)
00051 {
00052 Vector7d cam2world;
00053 for (int i=0; i<6; i++){
00054 is >> cam2world[i];
00055 }
00056 is >> cam2world[6];
00057
00058
00059
00060
00061
00062
00063 for (int i=0; i<2; i++)
00064 {
00065 is >> _focal_length[i];
00066 }
00067 for (int i=0; i<2; i++)
00068 {
00069 is >> _principle_point[i];
00070 }
00071
00072 estimate() = Sim3(cam2world).inverse();
00073 return true;
00074 }
00075
00076 bool VertexSim3Expmap::write(std::ostream& os) const
00077 {
00078 Sim3 cam2world(estimate().inverse());
00079 Vector7d lv=cam2world.log();
00080 for (int i=0; i<7; i++){
00081 os << lv[i] << " ";
00082 }
00083 for (int i=0; i<2; i++)
00084 {
00085 os << _focal_length[i] << " ";
00086 }
00087 for (int i=0; i<2; i++)
00088 {
00089 os << _principle_point[i] << " ";
00090 }
00091 return os.good();
00092 }
00093
00094 bool EdgeSim3::read(std::istream& is)
00095 {
00096 Vector7d v7;
00097 for (int i=0; i<7; i++){
00098 is >> v7[i];
00099 }
00100
00101 Sim3 cam2world(v7);
00102 measurement() = cam2world.inverse();
00103
00104 inverseMeasurement() = cam2world;
00105
00106 for (int i=0; i<7; i++)
00107 for (int j=i; j<7; j++)
00108 {
00109 is >> information()(i,j);
00110 if (i!=j)
00111 information()(j,i)=information()(i,j);
00112 }
00113 return true;
00114 }
00115
00116 bool EdgeSim3::write(std::ostream& os) const
00117 {
00118 Sim3 cam2world(measurement().inverse());
00119 Vector7d v7 = cam2world.log();
00120 for (int i=0; i<7; i++)
00121 {
00122 os << v7[i] << " ";
00123 }
00124 for (int i=0; i<7; i++)
00125 for (int j=i; j<7; j++){
00126 os << " " << information()(i,j);
00127 }
00128 return os.good();
00129 }
00130
00133 EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() :
00134 BaseBinaryEdge<2, Vector2d, VertexPointXYZ, VertexSim3Expmap>()
00135 {
00136 }
00137
00138 bool EdgeSim3ProjectXYZ::read(std::istream& is)
00139 {
00140 for (int i=0; i<2; i++)
00141 {
00142 is >> measurement()[i];
00143 }
00144
00145 inverseMeasurement() = -measurement();
00146 for (int i=0; i<2; i++)
00147 for (int j=i; j<2; j++) {
00148 is >> information()(i,j);
00149 if (i!=j)
00150 information()(j,i)=information()(i,j);
00151 }
00152 return true;
00153 }
00154
00155 bool EdgeSim3ProjectXYZ::write(std::ostream& os) const
00156 {
00157 for (int i=0; i<2; i++){
00158 os << measurement()[i] << " ";
00159 }
00160
00161 for (int i=0; i<2; i++)
00162 for (int j=i; j<2; j++){
00163 os << " " << information()(i,j);
00164 }
00165 return os.good();
00166 }
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211 }