public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add jacobian to development

Browse Source
This commit is contained in:
VahdPourAmin
2021-09-27 13:40:40 +02:00
parent 9d3ecd8f18
commit 414a3e4c10
9 changed files with 8071 additions and 1039 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/frame_handler_log.txt
+7987 -1026
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/point.h
+39
View File
@@ -104,6 +104,45 @@ public:
point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
point_jac = - point_jac * R_f_w;
}
void jacobian_xyz2uv_(
const Vector3d& p_in_f,
const Matrix3d& R_f_w,
Matrix23d& point_jac,
double * cam_params,
SE2_5 fram_t_f_w)
{
double fx = cam_params[0];
double fy = cam_params[1];
double s = cam_params[4];
double r = cam_params[5];
double x_n = p_in_f.x();
double y_n = p_in_f.y();
double z_n = p_in_f.z();
double x_c = fram_t_f_w.se3().translation().x();
double z_c = fram_t_f_w.se3().translation().y();
double theta = fram_t_f_w.pitch();
double alpha = (fx*(theta/r))-(fx*((x_n*x_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*x_n)/(r*r));
double beta = -(fx*((x_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*y_n)/(r*r));
double gamma = -(fy*((x_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fy*x_n*y_n)/(r*r));
double lamda = (fy*(theta/r))-(fy*((y_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fy*y_n*y_n)/(r*r));
double Xf_Xc = x_n - x_c;
double Zf_Zc = z_n - z_c;
double n1 = -1*sin(theta)*Xf_Xc + cos(theta)*Zf_Zc;
double n2 = -1*cos(theta)*Xf_Xc - sin(theta)*Zf_Zc;
point_jac(0, 0) = ((cos(theta)/z_n)*alpha)+(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*sin(theta));
point_jac(0, 1) = ((sin(theta)/z_n)*alpha)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*cos(theta));
point_jac(0, 2) = ((1/z_n)*alpha*n1)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n2);
point_jac(1, 0) = ((cos(theta)/z_n)*gamma)+(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*sin(theta));
point_jac(1, 1) = ((sin(theta)/z_n)*gamma)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*cos(theta));
point_jac(1, 2) = ((1/z_n)*gamma*n1)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
//point_jac = - point_jac * R_f_w;
}
};
} // namespace vio
GPU_version/vio/include/vio/reprojector.h
+3 -2
View File
@@ -66,7 +66,8 @@ public:
FramePtr frame,
FramePtr last_frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
opencl* gpu_fast_);
opencl* gpu_fast_,
FILE* log_);
private:
@@ -99,7 +100,7 @@ private:
static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
void initializeGrid(vk::AbstractCamera* cam);
void resetGrid();
bool reprojectCell(Cell& cell, FramePtr frame);
bool reprojectCell(Cell& cell, FramePtr frame, FILE* log_);
};
} // namespace vio
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/feature_alignment.cpp
+1
View File
@@ -216,6 +216,7 @@ bool align2D(
if(isnan(u) || isnan(v)) {
// TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
std::cerr<<"if is nan"<<std::endl;
return false;
}
GPU_version/vio/src/frame_handler_mono.cpp
+1 -1
View File
@@ -193,7 +193,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_=last_frame_;
return RESULT_FAILURE;
}
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
#if VIO_DEBUG
GPU_version/vio/src/matcher.cpp
+2 -2
View File
@@ -41,7 +41,7 @@ void getWarpMatrixAffine(
Matrix2d& A_cur_ref)
{
// Compute affine warp matrix A_ref_cur
const int halfpatch_size = 5;
const int halfpatch_size = 4;
const Vector3d xyz_ref(f_ref*depth_ref);
Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
@@ -160,7 +160,7 @@ bool Matcher::findMatchDirect(
// warp affine
warp::getWarpMatrixAffine(
*ref_ftr_->frame->cam_, *(cur_frame.cam_), ref_ftr_->px, ref_ftr_->f,
(Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),
(ref_ftr_->frame->se3().inverse()*pt.pos_).norm(),/*(Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),*/
cur_frame.se3().inverse() * ref_ftr_->frame->se3(), ref_ftr_->level, A_cur_ref_);
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
GPU_version/vio/src/point.cpp
+2 -1
View File
@@ -155,7 +155,8 @@ void Point::optimize(const size_t n_iter)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->se3()*pos_);
Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J);
//Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J);
Point::jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
GPU_version/vio/src/reprojector.cpp
+35 -6
View File
@@ -69,7 +69,8 @@ namespace vio {
FramePtr frame,
FramePtr last_frame,
std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs,
opencl* gpu_fast_) {
opencl* gpu_fast_,
FILE* log_) {
if(frame->id_<1)return;
resetGrid();
cv::Mat descriptors_cur;
@@ -142,13 +143,22 @@ namespace vio {
}
}
{
int cl = 0;
for (auto&& cell : grid_.cells) {
if (reprojectCell(*cell, frame))
#if VIO_DEBUG
if(cell->size()>=1) {
fprintf(log_, "[%s] %d-%d/%d : \n", vio::time_in_HH_MM_SS_MMM().c_str(), frame->id_, ++cl,
grid_.cells.size());
}
#endif
//std::cerr<<frame->id_<<"-"<<++cl<<"/"<<grid_.cells.size()<<" : ";
if (reprojectCell(*cell, frame, log_))
++n_matches_;
if (n_matches_ > (size_t) Config::maxFts())
break;
}
}
//std::exit(154);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -158,8 +168,11 @@ namespace vio {
return false;
}
bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
if(cell.size()<1)return false;
bool Reprojector::reprojectCell(Cell &cell, FramePtr frame, FILE* log_) {
//cv::Mat image = frame->img();
if(cell.size()<1){
return false;
}
cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
Cell::iterator it=cell.begin();
while(it!=cell.end())
@@ -175,8 +188,22 @@ namespace vio {
continue;
}
///TODO check point candidate handling
if(!matcher_.findMatchDirect(*it->pt, *frame, it->px))
//cv::Point pointb(it->px.x(), it->px.y());
//cv::circle(image, pointb,1,(255, 0, 0), 1);
Vector2d uvb(it->px.x(), it->px.y());
#if VIO_DEBUG
fprintf(log_,"befor : %f %f after : ", it->px.x(), it->px.y());
#endif
//std::cerr<<"before : "<<it->px.x()<<" "<<it->px.y()<<" after : ";
bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);
Vector2d uva(it->px.x(), it->px.y());
#if VIO_DEBUG
fprintf(log_,"%f %f %s %f\n", it->px.x(), it->px.y(), (res?" TRUE":" FALSE"), (uvb - uva).squaredNorm());
#endif
//std::cerr<<it->px.x()<<" "<<it->px.y()<<(res?" TRUE":" FALSE")<<std::endl;
//cv::Point pointa(it->px.x(), it->px.y());
//cv::circle(image, pointa,1,(0, 255, 0), 1);
if(!res)
{
it->pt->n_failed_reproj_++;
/* if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
@@ -214,6 +241,8 @@ namespace vio {
// Maximum one point per cell.
return true;
}
//cv::imwrite("/root/Projects/ROS_p_33_dev/src/p_33_vio/GPU_version/vio/image.png", image);
//std::exit(223);
return false;
}