void refineGraphSE3RgbdICP(const std::vector<Ptr<RgbdFrame> >& _frames,
const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks,
const Mat& cameraMatrix, float pointsPart,
std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
CV_Assert(_frames.size() == poses.size());
const int iterCount = 5;
// TODO: find corresp to main API?
// TODO: odom with one level here
RgbdICPOdometry odom;
vector<float> minGradientMagnitudes(1,10);
vector<int> iterCounts(1,10);
odom.set("maxPointsPart", pointsPart);
odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes).clone());
odom.set("iterCounts", Mat(iterCounts).clone());
odom.set("cameraMatrix", cameraMatrix);
std::vector<Ptr<OdometryFrame> > frames(_frames.size());
for(size_t i = 0; i < frames.size(); i++)
{
Mat gray;
CV_Assert(_frames[i]->image.channels() == 3);
cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
Ptr<OdometryFrame> frame = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
_frames[i]->normals, _frames[i]->ID);
odom.prepareFrameCache(frame, OdometryFrame::CACHE_ALL);
frames[i] = frame;
}
refinedPoses.resize(poses.size());
for(size_t i = 0; i < poses.size(); i++)
refinedPoses[i] = poses[i].clone();
Mat cameraMatrix_64F;
cameraMatrix.convertTo(cameraMatrix_64F, CV_64FC1);
for(int iter = 0; iter < iterCount; iter++)
{
G2OLinearSolver* linearSolver = createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);
fillGraphSE3RgbdICP(optimizer, frames, refinedPoses, posesLinks, cameraMatrix_64F, frameIndices);
optimizer->initializeOptimization();
const int optIterCount = 1;
cout << "Vertices count: " << optimizer->vertices().size() << endl;
cout << "Edges count: " << optimizer->edges().size() << endl;
cout << "Start optimization " << endl;
if(optimizer->optimize(optIterCount) != optIterCount)
{
optimizer->clear();
delete optimizer;
break;
}
cout << "Finish optimization " << endl;
getSE3Poses(optimizer, frameIndices, refinedPoses);
optimizer->clear();
delete optimizer;
}
}
void refineGraphSE3RgbdICPModel(std::vector<Ptr<RgbdFrame> >& _frames,
const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks, const Mat& cameraMatrix,
std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
CV_Assert(_frames.size() == poses.size());
g2o::Edge_V_V_GICPLandmark::initializeStaticMatrices(); // TODO: make this more correctly
Mat cameraMatrix_64F, cameraMatrix_inv_64F;
cameraMatrix.convertTo(cameraMatrix_64F, CV_64FC1);
cameraMatrix_inv_64F = cameraMatrix_64F.inv();
refinedPoses.resize(poses.size());
for(size_t i = 0; i < poses.size(); i++)
refinedPoses[i] = poses[i];
RgbdICPOdometry odom;
vector<float> minGradientMagnitudes(1,10);
vector<int> iterCounts(1,10);
odom.set("maxPointsPart", 1.);
odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes).clone());
odom.set("iterCounts", Mat(iterCounts).clone());
odom.set("cameraMatrix", cameraMatrix);
std::vector<Ptr<OdometryFrame> > frames(_frames.size());
for(size_t i = 0; i < frames.size(); i++)
{
//frames[i]->releasePyramids();
Mat gray;
CV_Assert(_frames[i]->image.channels() == 3);
cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
Ptr<OdometryFrame> fr = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
_frames[i]->normals, _frames[i]->ID);
odom.prepareFrameCache(fr, OdometryFrame::CACHE_ALL);
frames[i] = fr;
}
const double maxDepthDiff = 0.002;
for(size_t i = 0; i < frames.size(); i++)
{
frames[i]->releasePyramids();
odom.prepareFrameCache(frames[i], OdometryFrame::CACHE_ALL);
}
const int iterCount = 3;//7
const int minCorrespCount = 3;
const double maxTranslation = 0.20;
const double maxRotation = 30;
for(int iter = 0; iter < iterCount; iter++)
{
G2OLinearSolver* linearSolver = createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);
fillGraphSE3RgbdICP(optimizer, frames, refinedPoses, posesLinks, cameraMatrix_64F, frameIndices);
vector<Mat> vertexIndices(frameIndices.size());
int vertexIdx = frames.size();
for(size_t currIdx = 0; currIdx < frameIndices.size(); currIdx++)
{
int currFrameIdx = frameIndices[currIdx];
vertexIndices[currIdx] = Mat(frames[currFrameIdx]->image.size(), CV_32SC1, Scalar(-1));
Mat& curVertexIndices = vertexIndices[currIdx];
const Mat& curCloud = frames[currFrameIdx]->pyramidCloud[0];
const Mat& curNormals = frames[currFrameIdx]->normals;
// compute count of correspondences
Mat correspsCounts = Mat(frames[currFrameIdx]->image.size(), CV_32SC1, Scalar(0));
for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
{
int prevFrameIdx = frameIndices[prevIdx];
if(currFrameIdx == prevFrameIdx)
continue;
Mat curToPrevRt = refinedPoses[prevFrameIdx].inv(DECOMP_SVD) * refinedPoses[currFrameIdx];
if(tvecNorm(curToPrevRt) > maxTranslation || rvecNormDegrees(curToPrevRt) > maxRotation)
continue;
Mat transformedPrevNormals;
perspectiveTransform(frames[prevFrameIdx]->pyramidNormals[0], transformedPrevNormals,
curToPrevRt.inv(DECOMP_SVD));
Mat corresps;
computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_inv_64F, curToPrevRt.inv(DECOMP_SVD),
frames[currFrameIdx]->depth,
frames[currFrameIdx]->mask,
frames[prevFrameIdx]->depth,
frames[prevFrameIdx]->pyramidNormalsMask[0],
maxDepthDiff, corresps,
frames[currFrameIdx]->pyramidNormals[0],
transformedPrevNormals,
frames[currFrameIdx]->image,
frames[prevFrameIdx]->image);
for(int v0 = 0; v0 < corresps.rows; v0++)
{
for(int u0 = 0; u0 < corresps.cols; u0++)
{
int c = corresps.at<int>(v0, u0);
if(c != -1)
correspsCounts.at<int>(v0,u0)++;
}
}
}
// set up edges
for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
{
int prevFrameIdx = frameIndices[prevIdx];
if(currFrameIdx == prevFrameIdx)
continue;
const Mat& prevCloud = frames[prevFrameIdx]->pyramidCloud[0];
const Mat& prevNormals = frames[prevFrameIdx]->normals;
Mat curToPrevRt = refinedPoses[prevFrameIdx].inv(DECOMP_SVD) * refinedPoses[currFrameIdx];
if(tvecNorm(curToPrevRt) > maxTranslation || rvecNormDegrees(curToPrevRt) > maxRotation)
continue;
Mat transformedPrevNormals;
perspectiveTransform(frames[prevFrameIdx]->pyramidNormals[0], transformedPrevNormals,
curToPrevRt.inv(DECOMP_SVD));
Mat corresps;
computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_inv_64F, curToPrevRt.inv(DECOMP_SVD),
frames[currFrameIdx]->depth,
frames[currFrameIdx]->mask,
frames[prevFrameIdx]->depth,
frames[prevFrameIdx]->pyramidNormalsMask[0],
maxDepthDiff, corresps,
frames[currFrameIdx]->pyramidNormals[0],
transformedPrevNormals,
frames[currFrameIdx]->image,
frames[prevFrameIdx]->image);
std::cout << "landmarks: iter " << iter << "; cur " << currFrameIdx << "; prev " << prevFrameIdx << "; corresps " << countNonZero(corresps != -1) << std::endl;
// poses and edges for points3d
for(int v0 = 0; v0 < corresps.rows; v0++)
{
for(int u0 = 0; u0 < corresps.cols; u0++)
{
int c = corresps.at<int>(v0, u0);
if(c == -1)
continue;
if(correspsCounts.at<int>(v0,u0) < minCorrespCount)
continue;
int u1_, v1_;
get2shorts(c, u1_, v1_);
const Rect rect(0,0,curCloud.cols, curCloud.rows);
const int Rad = 0;
for(int v1 = v1_ - Rad; v1 <= v1_ + Rad; v1++)
{
for(int u1 = u1_ - Rad; u1 <= u1_ + Rad; u1++)
{
if(rect.contains(Point(u1, v1)) && !cvIsNaN(prevCloud.at<Point3f>(v1,u1).x) &&
std::abs(prevCloud.at<Point3f>(v1,u1).z - prevCloud.at<Point3f>(v1_,u1_).z) < maxDepthDiff)
{
Eigen::Vector3d pt_prev, pt_cur, norm_prev, norm_cur, global_norm_prev;
{
pt_prev = cvtPoint_ocv2egn(prevCloud.at<Point3f>(v1,u1));
norm_prev = cvtPoint_ocv2egn(prevNormals.at<Point3f>(v1,u1));
vector<Point3f> tp;
perspectiveTransform(vector<Point3f>(1, prevNormals.at<Point3f>(v1,u1)),
tp, refinedPoses[prevFrameIdx]);
CV_Assert(isValidDepth(tp[0].z));
tp[0] *= 1./cv::norm(tp[0]);
global_norm_prev = cvtPoint_ocv2egn(tp[0]);
perspectiveTransform(vector<Point3f>(1, curCloud.at<Point3f>(v0,u0)),
tp, refinedPoses[currFrameIdx]);
CV_Assert(isValidDepth(tp[0].z));
pt_cur = cvtPoint_ocv2egn(tp[0]);
perspectiveTransform(vector<Point3f>(1, curNormals.at<Point3f>(v0,u0)),
tp, refinedPoses[currFrameIdx]);
tp[0] *= 1./cv::norm(tp[0]);
CV_Assert(isValidDepth(tp[0].z));
norm_cur = cvtPoint_ocv2egn(tp[0]);
}
// add new pose
if(curVertexIndices.at<int>(v0,u0) == -1)
{
g2o::VertexPointXYZ* modelPoint = new g2o::VertexPointXYZ;
modelPoint->setId(vertexIdx);
modelPoint->setEstimate(pt_cur);
modelPoint->setMarginalized(true);
optimizer->addVertex(modelPoint);
curVertexIndices.at<int>(v0,u0) = vertexIdx;
vertexIdx++;
}
int vidx = curVertexIndices.at<int>(v0,u0);
g2o::Edge_V_V_GICPLandmark * e = new g2o::Edge_V_V_GICPLandmark();
e->setVertex(0, optimizer->vertex(vidx));
e->setVertex(1, optimizer->vertex(prevFrameIdx));
g2o::EdgeGICP meas;
meas.pos0 = pt_cur;
meas.pos1 = pt_prev;
meas.normal0 = norm_cur;
meas.normal1 = norm_prev;
e->setMeasurement(meas);
meas = e->measurement();
// e->information() = meas.prec0(0.01);
meas.normal1 = global_norm_prev; // to get global covariation
e->information() = 0.001 * (meas.cov0(1.).inverse() + meas.cov1(1.).inverse());
meas.normal1 = norm_prev; // set local normal
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
optimizer->addEdge(e);
}
}
}
}
}
}
}
optimizer->initializeOptimization();
const int optIterCount = 1;
cout << "Vertices count: " << optimizer->vertices().size() << endl;
cout << "Edges count: " << optimizer->edges().size() << endl;
if(optimizer->optimize(optIterCount) != optIterCount)
{
optimizer->clear();
delete optimizer;
break;
}
getSE3Poses(optimizer, frameIndices, refinedPoses);
// update points poses
cout << "Updating model points..." << endl;
CV_Assert(frameIndices.size() == vertexIndices.size());
for(size_t i = 0; i < frameIndices.size(); i++)
{
int frameIdx = frameIndices[i];
const Mat& curVertexIndices = vertexIndices[i];
Mat& depth = frames[frameIdx]->depth;
for(int y = 0; y < curVertexIndices.rows; y++)
{
for(int x = 0; x < curVertexIndices.cols; x++)
{
int vidx = curVertexIndices.at<int>(y,x);
if(vidx < 0)
continue;
Point3f p;
{
g2o::VertexPointXYZ* v = dynamic_cast<g2o::VertexPointXYZ*>(optimizer->vertex(vidx));
Eigen::Vector3d ep = v->estimate();
p.x = ep[0]; p.y = ep[1]; p.z = ep[2];
}
vector<Point3f> tp;
perspectiveTransform(vector<Point3f>(1, p), tp, refinedPoses[frameIdx].inv(DECOMP_SVD));
CV_Assert(isValidDepth(tp[0].z));
depth.at<float>(y,x) = tp[0].z;
}
}
frames[frameIdx]->pyramidMask.clear();
frames[frameIdx]->pyramidDepth.clear();
frames[frameIdx]->normals.release();
frames[frameIdx]->pyramidNormals.clear();
frames[frameIdx]->pyramidCloud.clear();
frames[frameIdx]->pyramidNormalsMask.clear();
odom.prepareFrameCache(frames[frameIdx], OdometryFrame::CACHE_ALL);
}
optimizer->clear();
delete optimizer;
}
// remove points without correspondences
for(size_t currIdx = 0; currIdx < frameIndices.size(); currIdx++)
{
int currFrameIdx = frameIndices[currIdx];
// compute count of correspondences
Mat& curCloud = frames[currFrameIdx]->pyramidCloud[0];
Mat& curDepth = frames[currFrameIdx]->depth;
Mat correspsCounts = Mat(frames[currFrameIdx]->image.size(), CV_32SC1, Scalar(0));
for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
{
int prevFrameIdx = frameIndices[prevIdx];
if(currFrameIdx == prevFrameIdx)
continue;
Mat curToPrevRt = refinedPoses[prevFrameIdx].inv(DECOMP_SVD) * refinedPoses[currFrameIdx];
Mat transformedPrevNormals;
perspectiveTransform(frames[prevFrameIdx]->pyramidNormals[0], transformedPrevNormals,
curToPrevRt.inv(DECOMP_SVD));
Mat corresps;
computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_inv_64F, curToPrevRt.inv(DECOMP_SVD),
frames[currFrameIdx]->depth,
frames[currFrameIdx]->mask,
frames[prevFrameIdx]->depth,
frames[prevFrameIdx]->pyramidNormalsMask[0],
0.003, corresps,
frames[currFrameIdx]->pyramidNormals[0],
transformedPrevNormals,
frames[currFrameIdx]->image,
frames[prevFrameIdx]->image);
for(int v0 = 0; v0 < corresps.rows; v0++)
{
for(int u0 = 0; u0 < corresps.cols; u0++)
{
int c = corresps.at<int>(v0, u0);
if(c != -1)
correspsCounts.at<int>(v0,u0)++;
}
}
}
for(int v0 = 0; v0 < correspsCounts.rows; v0++)
{
for(int u0 = 0; u0 < correspsCounts.cols; u0++)
{
if(correspsCounts.at<int>(v0,u0) < minCorrespCount)
{
curCloud.at<Point3f>(v0,u0) = Point3f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN());
}
curDepth.at<float>(v0,u0) = curCloud.at<Point3f>(v0,u0).z;
}
}
}
for(size_t i = 0; i < frames.size(); i++)
{
Ptr<RgbdFrame> fr = new RgbdFrame(_frames[i]->image, frames[i]->depth, _frames[i]->mask, frames[i]->normals, _frames[i]->ID);
_frames[i] = fr;
}
}
void refineGraphSE3RgbdICP(const std::vector<Ptr<RgbdFrame> >& _frames,
const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks,
const Mat& cameraMatrix, float pointsPart,
std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
CV_Assert(_frames.size() == poses.size());
// TODO: find corresp to main API?
const int levelsCount = 3;
vector<float> minGradientMagnitudes(levelsCount);
minGradientMagnitudes[0] = 10;
minGradientMagnitudes[1] = 5;
minGradientMagnitudes[2] = 1;
vector<int> iterCounts(levelsCount);
iterCounts[0] = 3;
iterCounts[1] = 4;
iterCounts[2] = 4;
RgbdICPOdometry odom;
odom.set("maxPointsPart", pointsPart);
odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes));
odom.set("iterCounts", Mat(iterCounts));
odom.set("cameraMatrix", cameraMatrix);
std::vector<Ptr<OdometryFrame> > frames(_frames.size());
for(size_t i = 0; i < frames.size(); i++)
{
Mat gray;
CV_Assert(_frames[i]->image.channels() == 3);
cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
Ptr<OdometryFrame> frame = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
_frames[i]->normals, _frames[i]->ID);
odom.prepareFrameCache(frame, OdometryFrame::CACHE_ALL);
frames[i] = frame;
}
refinedPoses.resize(poses.size());
for(size_t i = 0; i < poses.size(); i++)
refinedPoses[i] = poses[i].clone();
vector<Mat> pyramidCameraMatrix;
buildPyramidCameraMatrix(cameraMatrix, iterCounts.size(), pyramidCameraMatrix);
for(int level = iterCounts.size() - 1; level >= 0; level--)
{
for(int iter = 0; iter < iterCounts[level]; iter++)
{
G2OLinearSolver* linearSolver = createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);
double maxTranslation = DBL_MAX;
double maxRotation = DBL_MAX;
double maxDepthDiff = 0.07;
if(level == 0)
{
maxTranslation = 0.20;
maxRotation = 30;
}
fillGraphSE3RgbdICP(optimizer, level, frames, refinedPoses, posesLinks, pyramidCameraMatrix[level], frameIndices,
maxTranslation, maxRotation, maxDepthDiff);
optimizer->initializeOptimization();
const int optIterCount = 1;
cout << "Vertices count: " << optimizer->vertices().size() << endl;
cout << "Edges count: " << optimizer->edges().size() << endl;
cout << "Start optimization " << endl;
if(optimizer->optimize(optIterCount) != optIterCount)
{
optimizer->clear();
delete optimizer;
break;
}
cout << "Finish optimization " << endl;
getSE3Poses(optimizer, frameIndices, refinedPoses);
optimizer->clear();
delete optimizer;
}
}
}