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);
}
