void EdgeSE3PointXYZDepth::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/) { (void) from; assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ"); VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]); VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]); const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam(); Eigen::Vector3d p; p(2) = _measurement(2); p.head<2>() = _measurement.head<2>()*p(2); p=invKcam*p; point->setEstimate(cam->estimate() * (params->offsetMatrix() * p)); }
void EdgeSE3PointXYZDisparity::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) { (void) from; assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ"); VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]); VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]); // VertexCameraCache* vcache = (VertexCameraCache* ) cam->getCache(_cacheIds[0]); // if (! vcache){ // cerr << "fatal error in retrieving cache" << endl; // } //ParameterCamera* params=vcache->params; const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam(); Eigen::Vector3d p; double w=1./_measurement(2); p.head<2>() = _measurement.head<2>()*w; p(2) = w; p = invKcam * p; p = cam->estimate() * (params->offset() * p); point->setEstimate(p); }