add banchmark image alignment 2
This commit is contained in:
gfuse
@@ -0,0 +1,202 @@
|
||||
/*
|
||||
* math_utils.cpp
|
||||
*
|
||||
* Created on: Jul 20, 2012
|
||||
* Author: cforster
|
||||
*/
|
||||
|
||||
#include <math_utils.h>
|
||||
|
||||
namespace vk {
|
||||
|
||||
using namespace Eigen;
|
||||
|
||||
Vector3d
|
||||
triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
|
||||
const Vector3d& feature1, const Vector3d& feature2 )
|
||||
{
|
||||
Vector3d f2 = R * feature2;
|
||||
Vector2d b;
|
||||
b[0] = t.dot(feature1);
|
||||
b[1] = t.dot(f2);
|
||||
Matrix2d A;
|
||||
A(0,0) = feature1.dot(feature1);
|
||||
A(1,0) = feature1.dot(f2);
|
||||
A(0,1) = -A(1,0);
|
||||
A(1,1) = -f2.dot(f2);
|
||||
Vector2d lambda = A.inverse() * b;
|
||||
Vector3d xm = lambda[0] * feature1;
|
||||
Vector3d xn = t + lambda[1] * f2;
|
||||
return ( xm + xn )/2;
|
||||
}
|
||||
|
||||
bool
|
||||
depthFromTriangulationExact(
|
||||
const Matrix3d& R_r_c,
|
||||
const Vector3d& t_r_c,
|
||||
const Vector3d& f_r,
|
||||
const Vector3d& f_c,
|
||||
double& depth_in_r,
|
||||
double& depth_in_c)
|
||||
{
|
||||
// bearing vectors (f_r, f_c) do not need to be unit length
|
||||
const Vector3d f_c_in_r(R_r_c*f_c);
|
||||
const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r);
|
||||
const double b = f_c_in_r.dot(t_r_c);
|
||||
const double denom = (a*b - f_c_in_r.dot(f_c_in_r));
|
||||
|
||||
if(abs(denom) < 0.000001)
|
||||
return false;
|
||||
|
||||
depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom;
|
||||
depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm();
|
||||
return true;
|
||||
}
|
||||
|
||||
double
|
||||
reprojError(const Vector3d& f1,
|
||||
const Vector3d& f2,
|
||||
double error_multiplier2)
|
||||
{
|
||||
Vector2d e = project2d(f1) - project2d(f2);
|
||||
return error_multiplier2 * e.norm();
|
||||
}
|
||||
|
||||
double
|
||||
computeInliers(const vector<Vector3d>& features1, // c1
|
||||
const vector<Vector3d>& features2, // c2
|
||||
const Matrix3d& R, // R_c1_c2
|
||||
const Vector3d& t, // c1_t
|
||||
const double reproj_thresh,
|
||||
double error_multiplier2,
|
||||
vector<Vector3d>& xyz_vec, // in frame c1
|
||||
vector<int>& inliers,
|
||||
vector<int>& outliers)
|
||||
{
|
||||
inliers.clear(); inliers.reserve(features1.size());
|
||||
outliers.clear(); outliers.reserve(features1.size());
|
||||
xyz_vec.clear(); xyz_vec.reserve(features1.size());
|
||||
double tot_error = 0;
|
||||
//triangulate all features and compute reprojection errors and inliers
|
||||
for(size_t j=0; j<features1.size(); ++j)
|
||||
{
|
||||
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
|
||||
double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
|
||||
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
|
||||
if(e1 > reproj_thresh || e2 > reproj_thresh)
|
||||
outliers.push_back(j);
|
||||
else
|
||||
{
|
||||
inliers.push_back(j);
|
||||
tot_error += e1+e2;
|
||||
}
|
||||
}
|
||||
return tot_error;
|
||||
}
|
||||
|
||||
void
|
||||
computeInliersOneView(const vector<Vector3d> & feature_sphere_vec,
|
||||
const vector<Vector3d> & xyz_vec,
|
||||
const Matrix3d &R,
|
||||
const Vector3d &t,
|
||||
const double reproj_thresh,
|
||||
const double error_multiplier2,
|
||||
vector<int>& inliers,
|
||||
vector<int>& outliers)
|
||||
{
|
||||
inliers.clear(); inliers.reserve(xyz_vec.size());
|
||||
outliers.clear(); outliers.reserve(xyz_vec.size());
|
||||
for(size_t j = 0; j < xyz_vec.size(); j++ )
|
||||
{
|
||||
double e = reprojError(feature_sphere_vec[j],
|
||||
R.transpose() * ( xyz_vec[j] - t ),
|
||||
error_multiplier2);
|
||||
if(e < reproj_thresh)
|
||||
inliers.push_back(j);
|
||||
else
|
||||
outliers.push_back(j);
|
||||
}
|
||||
}
|
||||
|
||||
Vector3d
|
||||
dcm2rpy(const Matrix3d &R)
|
||||
{
|
||||
Vector3d rpy;
|
||||
rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) );
|
||||
if( fabs( rpy[1] - M_PI/2 ) < 0.00001 )
|
||||
{
|
||||
rpy[2] = 0;
|
||||
rpy[0] = -atan2( R(0,1), R(1,1) );
|
||||
}
|
||||
else
|
||||
{
|
||||
if( fabs( rpy[1] + M_PI/2 ) < 0.00001 )
|
||||
{
|
||||
rpy[2] = 0;
|
||||
rpy[0] = -atan2( R(0,1), R(1,1) );
|
||||
}
|
||||
else
|
||||
{
|
||||
rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) );
|
||||
rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) );
|
||||
}
|
||||
}
|
||||
return rpy;
|
||||
}
|
||||
|
||||
Matrix3d
|
||||
rpy2dcm(const Vector3d &rpy)
|
||||
{
|
||||
Matrix3d R1;
|
||||
R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0;
|
||||
R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]);
|
||||
R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1);
|
||||
|
||||
Matrix3d R2;
|
||||
R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]);
|
||||
R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0;
|
||||
R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0);
|
||||
|
||||
Matrix3d R3;
|
||||
R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0;
|
||||
R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0;
|
||||
R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0;
|
||||
|
||||
return R3 * R2 * R1;
|
||||
}
|
||||
|
||||
Quaterniond
|
||||
angax2quat(const Vector3d& n, const double& angle)
|
||||
{
|
||||
// n must be normalized!
|
||||
double s(sin(angle/2));
|
||||
return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s );
|
||||
}
|
||||
|
||||
|
||||
Matrix3d
|
||||
angax2dcm(const Vector3d& n, const double& angle)
|
||||
{
|
||||
// n must be normalized
|
||||
Matrix3d sqewn(sqew(n));
|
||||
return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle)));
|
||||
}
|
||||
|
||||
double
|
||||
sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2)
|
||||
{
|
||||
Vector3d v3Dash = unproject2d(v2Dash);
|
||||
Vector3d v3 = unproject2d(v2);
|
||||
|
||||
double dError = v3Dash.transpose() * Essential * v3;
|
||||
|
||||
Vector3d fv3 = Essential * v3;
|
||||
Vector3d fTv3Dash = Essential.transpose() * v3Dash;
|
||||
|
||||
Vector2d fv3Slice = fv3.head<2>();
|
||||
Vector2d fTv3DashSlice = fTv3Dash.head<2>();
|
||||
|
||||
return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice)));
|
||||
}
|
||||
|
||||
} // end namespace vk
|
||||
@@ -0,0 +1,166 @@
|
||||
/**
|
||||
* This file is part of dvo.
|
||||
*
|
||||
* Copyright 2012 Christian Kerl <[email protected]> (Technical University of Munich)
|
||||
* For more information see <http://vision.in.tum.de/data/software/dvo>.
|
||||
*
|
||||
* dvo is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* dvo is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with dvo. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <numeric>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <robust_cost.h>
|
||||
#include <math_utils.h>
|
||||
|
||||
namespace vk {
|
||||
namespace robust_cost {
|
||||
|
||||
const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f;
|
||||
const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f;
|
||||
|
||||
TDistributionScaleEstimator::
|
||||
TDistributionScaleEstimator(const float dof) :
|
||||
dof_(dof),
|
||||
initial_sigma_(INITIAL_SIGMA)
|
||||
{}
|
||||
|
||||
float TDistributionScaleEstimator::
|
||||
compute(std::vector<float>& errors) const
|
||||
{
|
||||
std::cerr<<"42\n";
|
||||
float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_);
|
||||
int num = 0;
|
||||
float lambda = initial_lamda;
|
||||
int iterations = 0;
|
||||
do
|
||||
{
|
||||
++iterations;
|
||||
initial_lamda = lambda;
|
||||
num = 0;
|
||||
lambda = 0.0f;
|
||||
|
||||
for(std::vector<float>::iterator it=errors.begin(); it!=errors.end(); ++it)
|
||||
{
|
||||
if(std::isfinite(*it))
|
||||
{
|
||||
++num;
|
||||
const float error2 = (*it)*(*it);
|
||||
lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) );
|
||||
}
|
||||
}
|
||||
lambda = float(num) / lambda;
|
||||
} while(std::abs(lambda - initial_lamda) > 1e-3);
|
||||
|
||||
return std::sqrt(1.0f / lambda);
|
||||
}
|
||||
|
||||
const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745
|
||||
|
||||
float MADScaleEstimator::
|
||||
compute(std::vector<float>& errors) const
|
||||
{
|
||||
std::cerr<<"74\n";
|
||||
// error must be in absolute values!
|
||||
return NORMALIZER * vk::getMedian(errors);
|
||||
}
|
||||
|
||||
float NormalDistributionScaleEstimator::
|
||||
compute(std::vector<float>& errors) const
|
||||
{
|
||||
std::cerr<<"82\n";
|
||||
const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size();
|
||||
float var = 0.0;
|
||||
std::for_each(errors.begin(), errors.end(), [&](const float d) {
|
||||
var += (d - mean) * (d - mean);
|
||||
});
|
||||
return std::sqrt(var); // return standard deviation
|
||||
}
|
||||
|
||||
const float TukeyWeightFunction::DEFAULT_B = 4.6851f;
|
||||
|
||||
TukeyWeightFunction::TukeyWeightFunction(const float b)
|
||||
{
|
||||
configure(b);
|
||||
}
|
||||
|
||||
float TukeyWeightFunction::value(const float& x) const
|
||||
{
|
||||
const float x_square = x * x;
|
||||
if(x_square <= b_square)
|
||||
{
|
||||
const float tmp = 1.0f - x_square / b_square;
|
||||
return tmp * tmp;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void TukeyWeightFunction::
|
||||
configure(const float& param)
|
||||
{
|
||||
b_square = param * param;
|
||||
}
|
||||
|
||||
const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f;
|
||||
|
||||
TDistributionWeightFunction::
|
||||
TDistributionWeightFunction(const float dof)
|
||||
{
|
||||
configure(dof);
|
||||
}
|
||||
|
||||
float TDistributionWeightFunction::
|
||||
value(const float & x) const
|
||||
{
|
||||
return ((dof_ + 1.0f) / (dof_ + (x * x)));
|
||||
}
|
||||
|
||||
void TDistributionWeightFunction::
|
||||
configure(const float& param)
|
||||
{
|
||||
dof_ = param;
|
||||
normalizer_ = dof_ / (dof_ + 1.0f);
|
||||
}
|
||||
|
||||
const float HuberWeightFunction::DEFAULT_K = 1.345f;
|
||||
|
||||
HuberWeightFunction::
|
||||
HuberWeightFunction(const float k)
|
||||
{
|
||||
configure(k);
|
||||
}
|
||||
|
||||
void HuberWeightFunction::
|
||||
configure(const float& param)
|
||||
{
|
||||
k = param;
|
||||
}
|
||||
|
||||
float HuberWeightFunction::
|
||||
value(const float& t) const
|
||||
{
|
||||
const float t_abs = std::abs(t);
|
||||
if(t_abs < k)
|
||||
return 1.0f;
|
||||
else
|
||||
return k / t_abs;
|
||||
}
|
||||
|
||||
} // namespace robust_cost
|
||||
} // namespace vk
|
||||
|
||||
|
||||
@@ -0,0 +1,268 @@
|
||||
// This file is part of SVO - Semi-direct Visual Odometry.
|
||||
//
|
||||
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
|
||||
// (Robotics and Perception Group, University of Zurich, Switzerland).
|
||||
//
|
||||
// SVO is free software: you can redistribute it and/or modify it under the
|
||||
// terms of the GNU General Public License as published by the Free Software
|
||||
// Foundation, either version 3 of the License, or any later version.
|
||||
//
|
||||
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
|
||||
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
|
||||
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU General Public License
|
||||
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
#include <sparse_img_align_gpu.h>
|
||||
|
||||
namespace vio {
|
||||
|
||||
SparseImgAlignGpu::SparseImgAlignGpu(
|
||||
int max_level, int min_level, int n_iter,
|
||||
Method method, bool verbose,opencl* residual) :
|
||||
max_level_(max_level),
|
||||
min_level_(min_level),
|
||||
residual_(residual)
|
||||
{
|
||||
n_iter_ = n_iter;
|
||||
n_iter_init_ = n_iter_;
|
||||
method_ = method;
|
||||
verbose_ = verbose;
|
||||
eps_ = 1e-10;
|
||||
// data = fopen((std::string(getenv("HOME"))+"/Project/data.txt").c_str(), "w+");
|
||||
}
|
||||
int SparseImgAlignGpu::feature_counter(std::string filename) {
|
||||
int counter = 0;
|
||||
std::string line;
|
||||
std::ifstream myfile (filename);
|
||||
if (myfile.is_open()) {
|
||||
getline(myfile, line);
|
||||
while(line.find("features") == std::string::npos)
|
||||
getline(myfile, line);
|
||||
while (!myfile.eof()) {
|
||||
getline(myfile, line);
|
||||
counter ++;
|
||||
}
|
||||
myfile.close();
|
||||
}
|
||||
else
|
||||
std::cout << "Unable to open file";
|
||||
return counter;
|
||||
}
|
||||
void SparseImgAlignGpu::data_reader(std::string filename, float *rp, float *cp, float *c, float *fts, float *fts_px) {
|
||||
std::string line;
|
||||
std::ifstream myfile (filename);
|
||||
if (myfile.is_open()) {
|
||||
while (true) {
|
||||
getline(myfile, line);
|
||||
if (line.find("ref_frame") != std::string::npos) {
|
||||
getline(myfile, line);
|
||||
std::string substring;
|
||||
std::stringstream ss(line);
|
||||
getline(ss, substring, ',');
|
||||
*(rp) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(rp + 1) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(rp + 2) = atof(substring.c_str());
|
||||
} else if (line.find("cur_frame") != std::string::npos) {
|
||||
getline(myfile, line);
|
||||
std::string substring;
|
||||
std::stringstream ss(line);
|
||||
getline(ss, substring, ',');
|
||||
*(cp) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(cp + 1) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(cp + 2) = atof(substring.c_str());
|
||||
} else if (line.find("camera") != std::string::npos) {
|
||||
getline(myfile, line);
|
||||
std::string substring;
|
||||
std::stringstream ss(line);
|
||||
getline(ss, substring, ',');
|
||||
*(c) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(c + 1) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(c + 2) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(c + 3) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(c + 4) = atof(substring.c_str());
|
||||
}
|
||||
else{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (line.find("features") != std::string::npos) {
|
||||
int fts_i = 0;
|
||||
int fts_px_i = 0;
|
||||
while (!myfile.eof()) {
|
||||
getline(myfile, line);
|
||||
std::string substring;
|
||||
std::stringstream ss(line);
|
||||
getline(ss, substring, ',');
|
||||
*(fts + (fts_i)) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(fts + (fts_i + 1)) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(fts + (fts_i + 2)) = atof(substring.c_str());
|
||||
fts_i += 3;
|
||||
getline(ss, substring, ',');
|
||||
*(fts_px + (fts_px_i)) = atof(substring.c_str());
|
||||
getline(ss, substring, ',');
|
||||
*(fts_px + (fts_px_i + 1)) = atof(substring.c_str());
|
||||
fts_px_i += 2;
|
||||
}
|
||||
}
|
||||
myfile.close();
|
||||
}
|
||||
else
|
||||
std::cout << "Unable to open file";
|
||||
return;
|
||||
}
|
||||
/*void draw(double* points, int f) {
|
||||
for (int level = 2; level <= 4; level++) {
|
||||
double scale = pow(2.0, -level);
|
||||
cv::Mat ref_img = cv::imread(std::string(TEST_DATA_DIR)+"/fer"+ std::to_string(level)+".png");
|
||||
for (int i = 0; i < f; i++) {
|
||||
cv::Point p;
|
||||
p.x = points[2*i] * scale;
|
||||
p.y = points[2*i+1] * scale;
|
||||
cv::circle(ref_img, p, 1, (0, 0, 255));
|
||||
}
|
||||
cv::imwrite("/root/Projects/p_25_vio_opencl/Vio_OpenCL/test/res/ref"+ std::to_string(level)+".png",ref_img);
|
||||
}
|
||||
}*/
|
||||
size_t SparseImgAlignGpu::run()
|
||||
{
|
||||
std::string datafilename = std::string(TEST_DATA_DIR)+"/data.txt";
|
||||
int fts_size = feature_counter(datafilename);
|
||||
float* fts = new float[3 * fts_size];
|
||||
float* fts_px = new float[2 * fts_size];
|
||||
reset();
|
||||
float camera[5];
|
||||
float ref_pos[3];
|
||||
float cur_pos[3];
|
||||
data_reader(datafilename, ref_pos, cur_pos, camera, fts, fts_px);
|
||||
cl_float3 ref_[1],cur_[1];
|
||||
ref_[0].x=ref_pos[0];
|
||||
ref_[0].y=ref_pos[1];
|
||||
ref_[0].z=ref_pos[2];
|
||||
cur_[0].x=cur_pos[0];
|
||||
cur_[0].y=cur_pos[1];
|
||||
cur_[0].z=cur_pos[2];
|
||||
residual_->reload_buf(0,2,cur_);
|
||||
residual_->reload_buf(0,3,ref_);
|
||||
cl_float3 features[fts_size];
|
||||
cl_float2 featue_px[fts_size];
|
||||
|
||||
for (int i = 0; i < fts_size; ++i) {
|
||||
features[i].x = fts[3*i];
|
||||
features[i].y = fts[3*i+1];
|
||||
features[i].z = fts[3*i+2];
|
||||
}
|
||||
for (int i = 0; i < fts_size; ++i) {
|
||||
featue_px[i].x = fts_px[2*i];
|
||||
featue_px[i].y = fts_px[2*i+1];
|
||||
}
|
||||
|
||||
feature_counter_ = fts_size; // is used to compute the index of the cached jacobian
|
||||
level_=4;
|
||||
residual_->reload_buf(0,4,features);
|
||||
residual_->reload_buf(0,5,featue_px);
|
||||
Sophus::SE2 T_cur(cur_pos[2],Eigen::Vector2d(cur_pos[0],cur_pos[1]));///TODO temporary, we can remove it
|
||||
for(level_ = max_level_; level_ >= min_level_; --level_)
|
||||
{
|
||||
cv::Mat cur_img = cv::imread(std::string(TEST_DATA_DIR)+"/cur"+ std::to_string(level_)+".png");
|
||||
cv::Mat ref_img = cv::imread(std::string(TEST_DATA_DIR)+"/fer"+ std::to_string(level_)+".png");
|
||||
residual_->reload_buf(0,0,cur_img);
|
||||
residual_->reload_buf(0,1,ref_img);
|
||||
residual_->write_buf(0,6,level_);
|
||||
mu_ = 1.0;
|
||||
optimize(T_cur);
|
||||
}
|
||||
cl_float3 pos[1]={0};
|
||||
residual_->read(0,2,pos);
|
||||
std::cerr<<"pos : "<<pos[0].x<<"\t"<<pos[0].y<<"\t"<<pos[0].z<<std::endl;
|
||||
std::cerr<<pos[0].x - ref_pos[0]<<"\t "<<pos[0].y- ref_pos[1]<<"\t "<<pos[0].z- ref_pos[2]<<std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
double SparseImgAlignGpu::computeResiduals(
|
||||
bool linearize_system,
|
||||
bool compute_weight_scale)
|
||||
{
|
||||
float H[9*300]={0};
|
||||
cl_float3 J[300]={0.0};
|
||||
float chi2_[300]={0.0};
|
||||
residual_->write_buf(0,11,scale_);
|
||||
residual_->reload_buf(0,10,chi2_);
|
||||
residual_->reload_buf(0,8,H);
|
||||
residual_->reload_buf(0,9,J);
|
||||
/// TODO GPU compute based on the feature
|
||||
residual_->run(0,feature_counter_);
|
||||
float error[300]={0.0};
|
||||
float chi[300]={0.0};
|
||||
residual_->read(0,10,feature_counter_,chi);
|
||||
residual_->read(0,7,feature_counter_,error);
|
||||
for(int i = 1; i < feature_counter_; ++i){
|
||||
error[0] += error[i];
|
||||
chi[0] += chi[i];
|
||||
}
|
||||
scale_ =error[0];
|
||||
return chi[0];
|
||||
}
|
||||
|
||||
int SparseImgAlignGpu::solve()
|
||||
{
|
||||
float H[300*9]={0.0};
|
||||
cl_float3 J[300]={0.0};
|
||||
residual_->read(0,8,feature_counter_*9,H);
|
||||
residual_->read(0,9,feature_counter_,J);
|
||||
for(int i = 1; i < feature_counter_; ++i){
|
||||
H[0] += H[i*9];
|
||||
H[1] += H[i*9+1];
|
||||
H[2] += H[i*9+2];
|
||||
H[3] += H[i*9+3];
|
||||
H[4] += H[i*9+4];
|
||||
H[5] += H[i*9+5];
|
||||
H[6] += H[i*9+6];
|
||||
H[7] += H[i*9+7];
|
||||
H[8] += H[i*9+8];
|
||||
J[0].x -= J[i].x;
|
||||
J[0].y -= J[i].y;
|
||||
J[0].z -= J[i].z;
|
||||
}
|
||||
double Hd[9]={(double)H[0],(double)H[1],(double)H[2],
|
||||
(double)H[3],(double)H[4],(double)H[5],
|
||||
(double)H[6],(double)H[7],(double)H[8]};
|
||||
double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
|
||||
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
|
||||
if((bool) std::isnan((double) x_[0]))
|
||||
return 0;
|
||||
return 1;
|
||||
}
|
||||
void SparseImgAlignGpu::update()
|
||||
{
|
||||
/// TODO the update situation may have a smarter solution
|
||||
/// TODO the update situation may have a smarter solution
|
||||
cl_float3 pos[1]={0.0,0.0,0.0};
|
||||
residual_->read(0,2,pos);
|
||||
Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_);
|
||||
pos[0].x=(float)update.translation()(0);
|
||||
pos[0].y=(float)update.translation()(1);
|
||||
pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
|
||||
residual_->reload_buf(0,2,pos);
|
||||
}
|
||||
|
||||
void SparseImgAlignGpu::startIteration()
|
||||
{}
|
||||
|
||||
void SparseImgAlignGpu::finishIteration()
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace vio
|
||||
|
||||
Reference in New Issue
Block a user