1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 Kurt Konolige 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #include "types_icp.h" 28 #include "g2o/core/factory.h" 29 #include "g2o/stuff/macros.h" 30 31 #include <iostream> 32 33 using namespace Eigen; 34 35 namespace g2o { 36 37 G2O_REGISTER_TYPE_GROUP(icp); 38 G2O_REGISTER_TYPE(EDGE_V_V_GICP, Edge_V_V_GICP); 39 40 namespace types_icp { 41 int initialized = 0; 42 init()43 void init() 44 { 45 if (types_icp::initialized) 46 return; 47 //cerr << "Calling " << __FILE__ << " " << __PRETTY_FUNCTION__ << endl; 48 49 Edge_V_V_GICP::dRidx << 0.0,0.0,0.0, 50 0.0,0.0,2.0, 51 0.0,-2.0,0.0; 52 Edge_V_V_GICP::dRidy << 0.0,0.0,-2.0, 53 0.0,0.0,0.0, 54 2.0,0.0,0.0; 55 Edge_V_V_GICP::dRidz << 0.0,2.0,0.0, 56 -2.0,0.0,0.0, 57 0.0,0.0,0.0; 58 59 VertexSCam::dRidx << 0.0,0.0,0.0, 60 0.0,0.0,2.0, 61 0.0,-2.0,0.0; 62 VertexSCam::dRidy << 0.0,0.0,-2.0, 63 0.0,0.0,0.0, 64 2.0,0.0,0.0; 65 VertexSCam::dRidz << 0.0,2.0,0.0, 66 -2.0,0.0,0.0, 67 0.0,0.0,0.0; 68 69 types_icp::initialized = 1; 70 } 71 } 72 73 using namespace std; 74 75 Matrix3 Edge_V_V_GICP::dRidx; // differential quat matrices 76 Matrix3 Edge_V_V_GICP::dRidy; // differential quat matrices 77 Matrix3 Edge_V_V_GICP::dRidz; // differential quat matrices 78 Matrix3 VertexSCam::dRidx; // differential quat matrices 79 Matrix3 VertexSCam::dRidy; // differential quat matrices 80 Matrix3 VertexSCam::dRidz; // differential quat matrices 81 Matrix3 VertexSCam::Kcam; 82 number_t VertexSCam::baseline; 83 84 // global initialization G2O_ATTRIBUTE_CONSTRUCTOR(init_icp_types)85 G2O_ATTRIBUTE_CONSTRUCTOR(init_icp_types) 86 { 87 types_icp::init(); 88 } 89 90 // Copy constructor Edge_V_V_GICP(const Edge_V_V_GICP * e)91 Edge_V_V_GICP::Edge_V_V_GICP(const Edge_V_V_GICP* e) 92 : BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3>() 93 { 94 95 // Temporary hack - TODO, sort out const-ness properly 96 _vertices[0] = const_cast<HyperGraph::Vertex*> (e->vertex(0)); 97 _vertices[1] = const_cast<HyperGraph::Vertex*> (e->vertex(1)); 98 99 _measurement.pos0 = e->measurement().pos0; 100 _measurement.pos1 = e->measurement().pos1; 101 _measurement.normal0 = e->measurement().normal0; 102 _measurement.normal1 = e->measurement().normal1; 103 _measurement.R0 = e->measurement().R0; 104 _measurement.R1 = e->measurement().R1; 105 106 pl_pl = e->pl_pl; 107 cov0 = e->cov0; 108 cov1 = e->cov1; 109 110 // TODO the robust kernel is not correctly copied 111 //_robustKernel = e->_robustKernel; 112 } 113 114 // 115 // Rigid 3D constraint between poses, given fixed point offsets 116 // 117 118 // input two matched points between the frames 119 // first point belongs to the first frame, position and normal 120 // second point belongs to the second frame, position and normal 121 // 122 // the measurement variable has type EdgeGICP (see types_icp.h) 123 read(std::istream & is)124 bool Edge_V_V_GICP::read(std::istream& is) 125 { 126 // measured point and normal 127 for (int i=0; i<3; i++) 128 is >> _measurement.pos0[i]; 129 for (int i=0; i<3; i++) 130 is >> _measurement.normal0[i]; 131 132 // measured point and normal 133 for (int i=0; i<3; i++) 134 is >> _measurement.pos1[i]; 135 for (int i=0; i<3; i++) 136 is >> _measurement.normal1[i]; 137 138 // don't need this if we don't use it in error calculation (???) 139 // inverseMeasurement() = -measurement(); 140 141 _measurement.makeRot0(); // set up rotation matrices 142 143 // GICP info matrices 144 145 // point-plane only 146 Matrix3 prec; 147 number_t v = cst(.01); 148 prec << v, 0, 0, 149 0, v, 0, 150 0, 0, 1; 151 const Matrix3 &R = measurement().R0; // plane of the point in vp0 152 information() = R.transpose()*prec*R; 153 154 // information().setIdentity(); 155 156 // setRobustKernel(true); 157 //setHuberWidth(0.01); // units? m? 158 159 return true; 160 } 161 162 163 // Jacobian 164 // [ -R0'*R1 | R0 * dRdx/ddx * 0p1 ] 165 // [ R0'*R1 | R0 * dR'dx/ddx * 0p1 ] 166 167 #ifdef GICP_ANALYTIC_JACOBIANS 168 169 // jacobian defined as: 170 // f(T0,T1) = dR0.inv() * T0.inv() * (T1 * dR1 * p1 + dt1) - dt0 171 // df/dx0 = [-I, d[dR0.inv()]/dq0 * T01 * p1] 172 // df/dx1 = [R0, T01 * d[dR1]/dq1 * p1] linearizeOplus()173 void Edge_V_V_GICP::linearizeOplus() 174 { 175 VertexSE3* vp0 = static_cast<VertexSE3*>(_vertices[0]); 176 VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]); 177 178 // topLeftCorner<3,3>() is the rotation matrix 179 Matrix3 R0T = vp0->estimate().matrix().topLeftCorner<3,3>().transpose(); 180 Vector3 p1 = measurement().pos1; 181 182 // this could be more efficient 183 if (!vp0->fixed()) 184 { 185 Isometry3 T01 = vp0->estimate().inverse() * vp1->estimate(); 186 Vector3 p1t = T01 * p1; 187 _jacobianOplusXi.block<3,3>(0,0) = -Matrix3::Identity(); 188 _jacobianOplusXi.block<3,1>(0,3) = dRidx*p1t; 189 _jacobianOplusXi.block<3,1>(0,4) = dRidy*p1t; 190 _jacobianOplusXi.block<3,1>(0,5) = dRidz*p1t; 191 } 192 193 if (!vp1->fixed()) 194 { 195 Matrix3 R1 = vp1->estimate().matrix().topLeftCorner<3,3>(); 196 R0T = R0T*R1; 197 _jacobianOplusXj.block<3,3>(0,0) = R0T; 198 _jacobianOplusXj.block<3,1>(0,3) = R0T*dRidx.transpose()*p1; 199 _jacobianOplusXj.block<3,1>(0,4) = R0T*dRidy.transpose()*p1; 200 _jacobianOplusXj.block<3,1>(0,5) = R0T*dRidz.transpose()*p1; 201 } 202 } 203 #endif 204 205 write(std::ostream & os) const206 bool Edge_V_V_GICP::write(std::ostream& os) const 207 { 208 // first point 209 for (int i=0; i<3; i++) 210 os << measurement().pos0[i] << " "; 211 for (int i=0; i<3; i++) 212 os << measurement().normal0[i] << " "; 213 214 // second point 215 for (int i=0; i<3; i++) 216 os << measurement().pos1[i] << " "; 217 for (int i=0; i<3; i++) 218 os << measurement().normal1[i] << " "; 219 220 221 return os.good(); 222 } 223 224 // 225 // stereo camera functions 226 // 227 228 229 VertexSCam()230 VertexSCam::VertexSCam() : 231 VertexSE3() 232 {} 233 234 Edge_XYZ_VSC()235 Edge_XYZ_VSC::Edge_XYZ_VSC() 236 {} 237 238 #ifdef SCAM_ANALYTIC_JACOBIANS 239 /** 240 * \brief Jacobian for stereo projection 241 */ linearizeOplus()242 void Edge_XYZ_VSC::linearizeOplus() 243 { 244 VertexSCam *vc = static_cast<VertexSCam *>(_vertices[1]); 245 246 VertexSBAPointXYZ *vp = static_cast<VertexSBAPointXYZ *>(_vertices[0]); 247 Vector4 pt, trans; 248 pt.head<3>() = vp->estimate(); 249 pt(3) = 1.0; 250 trans.head<3>() = vc->estimate().translation(); 251 trans(3) = 1.0; 252 253 // first get the world point in camera coords 254 Eigen::Matrix<number_t,3,1,Eigen::ColMajor> pc = vc->w2n * pt; 255 256 // Jacobians wrt camera parameters 257 // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 258 number_t px = pc(0); 259 number_t py = pc(1); 260 number_t pz = pc(2); 261 number_t ipz2 = 1.0/(pz*pz); 262 if (isnan(ipz2) ) 263 { 264 std::cout << "[SetJac] infinite jac" << std::endl; 265 *(int *)0x0 = 0; 266 } 267 268 number_t ipz2fx = ipz2*vc->Kcam(0,0); // Fx 269 number_t ipz2fy = ipz2*vc->Kcam(1,1); // Fy 270 number_t b = vc->baseline; // stereo baseline 271 272 Eigen::Matrix<number_t,3,1,Eigen::ColMajor> pwt; 273 274 // check for local vars 275 pwt = (pt-trans).head<3>(); // transform translations, use differential rotation 276 277 // dx 278 Eigen::Matrix<number_t,3,1,Eigen::ColMajor> dp = vc->dRdx * pwt; // dR'/dq * [pw - t] 279 _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx; 280 _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy; 281 _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 282 // dy 283 dp = vc->dRdy * pwt; // dR'/dq * [pw - t] 284 _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx; 285 _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy; 286 _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 287 // dz 288 dp = vc->dRdz * pwt; // dR'/dq * [pw - t] 289 _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx; 290 _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy; 291 _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 292 293 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 294 dp = -vc->w2n.col(0); // dpc / dx 295 _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 296 _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 297 _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 298 dp = -vc->w2n.col(1); // dpc / dy 299 _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 300 _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 301 _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 302 dp = -vc->w2n.col(2); // dpc / dz 303 _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 304 _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 305 _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 306 307 // Jacobians wrt point parameters 308 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 309 dp = vc->w2n.col(0); // dpc / dx 310 _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 311 _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 312 _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 313 dp = vc->w2n.col(1); // dpc / dy 314 _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 315 _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 316 _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 317 dp = vc->w2n.col(2); // dpc / dz 318 _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 319 _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 320 _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 321 } 322 #endif read(std::istream &)323 bool Edge_XYZ_VSC::read(std::istream&) 324 { return false; } 325 write(std::ostream &) const326 bool Edge_XYZ_VSC::write(std::ostream&) const 327 { return false; } 328 read(std::istream &)329 bool VertexSCam::read(std::istream&) 330 { return false; } 331 write(std::ostream &) const332 bool VertexSCam::write(std::ostream&) const 333 { return false; } 334 335 } // end namespace 336