00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "types_icp.h"
00018 #include "g2o/core/factory.h"
00019
00020 #include <iostream>
00021
00022 namespace g2o {
00023
00024 using namespace std;
00025 using namespace Eigen;
00026 typedef Matrix<double, 6, 1> Vector6d;
00027
00028 Matrix3d Edge_V_V_GICP::dRidx;
00029 Matrix3d Edge_V_V_GICP::dRidy;
00030 Matrix3d Edge_V_V_GICP::dRidz;
00031 Matrix3d VertexSCam::dRidx;
00032 Matrix3d VertexSCam::dRidy;
00033 Matrix3d VertexSCam::dRidz;
00034 Matrix3d VertexSCam::Kcam;
00035 double VertexSCam::baseline;
00036
00037
00038 void __attribute__ ((constructor)) init_icp_types(void)
00039 {
00040
00041 Factory* factory = Factory::instance();
00042 factory->registerType("EDGE_V_V_GICP", new HyperGraphElementCreator<Edge_V_V_GICP>);
00043
00044 Edge_V_V_GICP::dRidx << 0.0,0.0,0.0,
00045 0.0,0.0,2.0,
00046 0.0,-2.0,0.0;
00047 Edge_V_V_GICP::dRidy << 0.0,0.0,-2.0,
00048 0.0,0.0,0.0,
00049 2.0,0.0,0.0;
00050 Edge_V_V_GICP::dRidz << 0.0,2.0,0.0,
00051 -2.0,0.0,0.0,
00052 0.0,0.0,0.0;
00053
00054 VertexSCam::dRidx << 0.0,0.0,0.0,
00055 0.0,0.0,2.0,
00056 0.0,-2.0,0.0;
00057 VertexSCam::dRidy << 0.0,0.0,-2.0,
00058 0.0,0.0,0.0,
00059 2.0,0.0,0.0;
00060 VertexSCam::dRidz << 0.0,2.0,0.0,
00061 -2.0,0.0,0.0,
00062 0.0,0.0,0.0;
00063
00064 }
00065
00066
00067 Edge_V_V_GICP::Edge_V_V_GICP(const Edge_V_V_GICP* e)
00068 : BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3>()
00069 {
00070 vertices()[0] = e->vertices()[0];
00071 vertices()[1] = e->vertices()[1];
00072
00073 measurement().pos0 = e->measurement().pos0;
00074 measurement().pos1 = e->measurement().pos1;
00075 measurement().normal0 = e->measurement().normal0;
00076 measurement().normal1 = e->measurement().normal1;
00077 measurement().R0 = e->measurement().R0;
00078 measurement().R1 = e->measurement().R1;
00079
00080 pl_pl = e->pl_pl;
00081 cov0 = e->cov0;
00082 cov1 = e->cov1;
00083
00084 _robustKernel = e->_robustKernel;
00085 _huberWidth = e->_huberWidth;
00086 }
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098 bool Edge_V_V_GICP::read(std::istream& is)
00099 {
00100
00101 for (int i=0; i<3; i++)
00102 is >> measurement().pos0[i];
00103 for (int i=0; i<3; i++)
00104 is >> measurement().normal0[i];
00105
00106
00107 for (int i=0; i<3; i++)
00108 is >> measurement().pos1[i];
00109 for (int i=0; i<3; i++)
00110 is >> measurement().normal1[i];
00111
00112
00113
00114
00115 measurement().makeRot0();
00116
00117
00118
00119
00120 Matrix3d prec;
00121 double v = .01;
00122 prec << v, 0, 0,
00123 0, v, 0,
00124 0, 0, 1;
00125 Matrix3d &R = measurement().R0;
00126 information() = R.transpose()*prec*R;
00127
00128
00129
00130
00131 setHuberWidth(0.01);
00132
00133 return true;
00134 }
00135
00136
00137
00138
00139
00140
00141 #ifdef GICP_ANALYTIC_JACOBIANS
00142
00143
00144
00145
00146
00147 void Edge_V_V_GICP::linearizeOplus()
00148 {
00149 VertexSE3* vp0 = static_cast<VertexSE3*>(_vertices[0]);
00150 VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]);
00151
00152 Matrix3d R0T = vp0->estimate().rotation().toRotationMatrix().transpose();
00153 Vector3d p1 = measurement().pos1;
00154
00155
00156 if (!vp0->fixed())
00157 {
00158 SE3Quat T01 = vp0->estimate().inverse() * vp1->estimate();
00159 Vector3d p1t = T01.map(p1);
00160 _jacobianOplusXi.block<3,3>(0,0) = -Matrix3d::Identity();
00161 _jacobianOplusXi.block<3,1>(0,3) = dRidx*p1t;
00162 _jacobianOplusXi.block<3,1>(0,4) = dRidy*p1t;
00163 _jacobianOplusXi.block<3,1>(0,5) = dRidz*p1t;
00164 }
00165
00166 if (!vp1->fixed())
00167 {
00168 Matrix3d R1 = vp1->estimate().rotation().toRotationMatrix();
00169 R0T = R0T*R1;
00170 _jacobianOplusXj.block<3,3>(0,0) = R0T;
00171 _jacobianOplusXj.block<3,1>(0,3) = R0T*dRidx.transpose()*p1;
00172 _jacobianOplusXj.block<3,1>(0,4) = R0T*dRidy.transpose()*p1;
00173 _jacobianOplusXj.block<3,1>(0,5) = R0T*dRidz.transpose()*p1;
00174 }
00175 }
00176 #endif
00177
00178
00179 bool Edge_V_V_GICP::write(std::ostream& os) const
00180 {
00181
00182 for (int i=0; i<3; i++)
00183 os << measurement().pos0[i] << " ";
00184 for (int i=0; i<3; i++)
00185 os << measurement().normal0[i] << " ";
00186
00187
00188 for (int i=0; i<3; i++)
00189 os << measurement().pos1[i] << " ";
00190 for (int i=0; i<3; i++)
00191 os << measurement().normal1[i] << " ";
00192
00193
00194 return os.good();
00195 }
00196
00197
00198
00199
00200
00201
00202
00203 VertexSCam::VertexSCam() :
00204 VertexSE3()
00205 {}
00206
00207
00208 Edge_XYZ_VSC::Edge_XYZ_VSC()
00209 {}
00210
00211 #ifdef SCAM_ANALYTIC_JACOBIANS
00212
00215 void Edge_XYZ_VSC::linearizeOplus()
00216 {
00217 VertexSCam *vc = static_cast<VertexSCam *>(_vertices[1]);
00218
00219 VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00220 Vector4d pt, trans;
00221 pt.head<3>() = vp->estimate();
00222 pt(3) = 1.0;
00223 trans.head<3>() = vc->estimate().translation();
00224 trans(3) = 1.0;
00225
00226
00227 Eigen::Matrix<double,3,1> pc = vc->w2n * pt;
00228
00229
00230
00231 double px = pc(0);
00232 double py = pc(1);
00233 double pz = pc(2);
00234 double ipz2 = 1.0/(pz*pz);
00235 if (isnan(ipz2) )
00236 {
00237 std::cout << "[SetJac] infinite jac" << std::endl;
00238 *(int *)0x0 = 0;
00239 }
00240
00241 double ipz2fx = ipz2*vc->Kcam(0,0);
00242 double ipz2fy = ipz2*vc->Kcam(1,1);
00243 double b = vc->baseline;
00244
00245 Eigen::Matrix<double,3,1> pwt;
00246
00247
00248 pwt = (pt-trans).head<3>();
00249
00250
00251 Eigen::Matrix<double,3,1> dp = vc->dRdx * pwt;
00252 _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00253 _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00254 _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00255
00256 dp = vc->dRdy * pwt;
00257 _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00258 _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00259 _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00260
00261 dp = vc->dRdz * pwt;
00262 _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00263 _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00264 _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00265
00266
00267 dp = -vc->w2n.col(0);
00268 _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00269 _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00270 _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00271 dp = -vc->w2n.col(1);
00272 _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00273 _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00274 _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00275 dp = -vc->w2n.col(2);
00276 _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00277 _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00278 _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00279
00280
00281
00282 dp = vc->w2n.col(0);
00283 _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00284 _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00285 _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00286 dp = vc->w2n.col(1);
00287 _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00288 _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00289 _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00290 dp = vc->w2n.col(2);
00291 _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00292 _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00293 _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00294 }
00295 #endif
00296 bool Edge_XYZ_VSC::read(std::istream&)
00297 { return false; }
00298
00299 bool Edge_XYZ_VSC::write(std::ostream&) const
00300 { return false; }
00301
00302 bool VertexSCam::read(std::istream&)
00303 { return false; }
00304
00305 bool VertexSCam::write(std::ostream&) const
00306 { return false; }
00307
00308 }