debug
This commit is contained in:
@@ -1,312 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<!DOCTYPE ipe SYSTEM "ipe.dtd">
|
|
||||||
<ipe version="70005" creator="Ipe 7.1.1">
|
|
||||||
<info created="D:20120312120943" modified="D:20140611144527"/>
|
|
||||||
<ipestyle name="basic">
|
|
||||||
<symbol name="arrow/arc(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="sym-stroke" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/farc(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="white" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/circle(sx)" transformations="translations">
|
|
||||||
<path fill="sym-stroke">
|
|
||||||
0.6 0 0 0.6 0 0 e
|
|
||||||
0.4 0 0 0.4 0 0 e
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/disk(sx)" transformations="translations">
|
|
||||||
<path fill="sym-stroke">
|
|
||||||
0.6 0 0 0.6 0 0 e
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/fdisk(sfx)" transformations="translations">
|
|
||||||
<group>
|
|
||||||
<path fill="sym-stroke" fillrule="eofill">
|
|
||||||
0.6 0 0 0.6 0 0 e
|
|
||||||
0.4 0 0 0.4 0 0 e
|
|
||||||
</path>
|
|
||||||
<path fill="sym-fill">
|
|
||||||
0.4 0 0 0.4 0 0 e
|
|
||||||
</path>
|
|
||||||
</group>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/box(sx)" transformations="translations">
|
|
||||||
<path fill="sym-stroke" fillrule="eofill">
|
|
||||||
-0.6 -0.6 m
|
|
||||||
0.6 -0.6 l
|
|
||||||
0.6 0.6 l
|
|
||||||
-0.6 0.6 l
|
|
||||||
h
|
|
||||||
-0.4 -0.4 m
|
|
||||||
0.4 -0.4 l
|
|
||||||
0.4 0.4 l
|
|
||||||
-0.4 0.4 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/square(sx)" transformations="translations">
|
|
||||||
<path fill="sym-stroke">
|
|
||||||
-0.6 -0.6 m
|
|
||||||
0.6 -0.6 l
|
|
||||||
0.6 0.6 l
|
|
||||||
-0.6 0.6 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/fsquare(sfx)" transformations="translations">
|
|
||||||
<group>
|
|
||||||
<path fill="sym-stroke" fillrule="eofill">
|
|
||||||
-0.6 -0.6 m
|
|
||||||
0.6 -0.6 l
|
|
||||||
0.6 0.6 l
|
|
||||||
-0.6 0.6 l
|
|
||||||
h
|
|
||||||
-0.4 -0.4 m
|
|
||||||
0.4 -0.4 l
|
|
||||||
0.4 0.4 l
|
|
||||||
-0.4 0.4 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
<path fill="sym-fill">
|
|
||||||
-0.4 -0.4 m
|
|
||||||
0.4 -0.4 l
|
|
||||||
0.4 0.4 l
|
|
||||||
-0.4 0.4 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</group>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="mark/cross(sx)" transformations="translations">
|
|
||||||
<group>
|
|
||||||
<path fill="sym-stroke">
|
|
||||||
-0.43 -0.57 m
|
|
||||||
0.57 0.43 l
|
|
||||||
0.43 0.57 l
|
|
||||||
-0.57 -0.43 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
<path fill="sym-stroke">
|
|
||||||
-0.43 0.57 m
|
|
||||||
0.57 -0.43 l
|
|
||||||
0.43 -0.57 l
|
|
||||||
-0.57 0.43 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</group>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/fnormal(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="white" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/pointed(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="sym-stroke" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-0.8 0 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/fpointed(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="white" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-0.8 0 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/linear(spx)">
|
|
||||||
<path stroke="sym-stroke" pen="sym-pen">
|
|
||||||
-1 0.333 m
|
|
||||||
0 0 l
|
|
||||||
-1 -0.333 l
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/fdouble(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="white" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
-1 0 m
|
|
||||||
-2 0.333 l
|
|
||||||
-2 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<symbol name="arrow/double(spx)">
|
|
||||||
<path stroke="sym-stroke" fill="sym-stroke" pen="sym-pen">
|
|
||||||
0 0 m
|
|
||||||
-1 0.333 l
|
|
||||||
-1 -0.333 l
|
|
||||||
h
|
|
||||||
-1 0 m
|
|
||||||
-2 0.333 l
|
|
||||||
-2 -0.333 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
</symbol>
|
|
||||||
<pen name="heavier" value="0.8"/>
|
|
||||||
<pen name="fat" value="1.2"/>
|
|
||||||
<pen name="ultrafat" value="2"/>
|
|
||||||
<symbolsize name="large" value="5"/>
|
|
||||||
<symbolsize name="small" value="2"/>
|
|
||||||
<symbolsize name="tiny" value="1.1"/>
|
|
||||||
<arrowsize name="large" value="10"/>
|
|
||||||
<arrowsize name="small" value="5"/>
|
|
||||||
<arrowsize name="tiny" value="3"/>
|
|
||||||
<color name="red" value="1 0 0"/>
|
|
||||||
<color name="green" value="0 1 0"/>
|
|
||||||
<color name="blue" value="0 0 1"/>
|
|
||||||
<color name="yellow" value="1 1 0"/>
|
|
||||||
<color name="orange" value="1 0.647 0"/>
|
|
||||||
<color name="gold" value="1 0.843 0"/>
|
|
||||||
<color name="purple" value="0.627 0.125 0.941"/>
|
|
||||||
<color name="gray" value="0.745"/>
|
|
||||||
<color name="brown" value="0.647 0.165 0.165"/>
|
|
||||||
<color name="navy" value="0 0 0.502"/>
|
|
||||||
<color name="pink" value="1 0.753 0.796"/>
|
|
||||||
<color name="seagreen" value="0.18 0.545 0.341"/>
|
|
||||||
<color name="turquoise" value="0.251 0.878 0.816"/>
|
|
||||||
<color name="violet" value="0.933 0.51 0.933"/>
|
|
||||||
<color name="darkblue" value="0 0 0.545"/>
|
|
||||||
<color name="darkcyan" value="0 0.545 0.545"/>
|
|
||||||
<color name="darkgray" value="0.663"/>
|
|
||||||
<color name="darkgreen" value="0 0.392 0"/>
|
|
||||||
<color name="darkmagenta" value="0.545 0 0.545"/>
|
|
||||||
<color name="darkorange" value="1 0.549 0"/>
|
|
||||||
<color name="darkred" value="0.545 0 0"/>
|
|
||||||
<color name="lightblue" value="0.678 0.847 0.902"/>
|
|
||||||
<color name="lightcyan" value="0.878 1 1"/>
|
|
||||||
<color name="lightgray" value="0.827"/>
|
|
||||||
<color name="lightgreen" value="0.565 0.933 0.565"/>
|
|
||||||
<color name="lightyellow" value="1 1 0.878"/>
|
|
||||||
<dashstyle name="dashed" value="[4] 0"/>
|
|
||||||
<dashstyle name="dotted" value="[1 3] 0"/>
|
|
||||||
<dashstyle name="dash dotted" value="[4 2 1 2] 0"/>
|
|
||||||
<dashstyle name="dash dot dotted" value="[4 2 1 2 1 2] 0"/>
|
|
||||||
<textsize name="large" value="\large"/>
|
|
||||||
<textsize name="Large" value="\Large"/>
|
|
||||||
<textsize name="LARGE" value="\LARGE"/>
|
|
||||||
<textsize name="huge" value="\huge"/>
|
|
||||||
<textsize name="Huge" value="\Huge"/>
|
|
||||||
<textsize name="small" value="\small"/>
|
|
||||||
<textsize name="footnote" value="\footnotesize"/>
|
|
||||||
<textsize name="tiny" value="\tiny"/>
|
|
||||||
<textstyle name="center" begin="\begin{center}" end="\end{center}"/>
|
|
||||||
<textstyle name="itemize" begin="\begin{itemize}" end="\end{itemize}"/>
|
|
||||||
<textstyle name="item" begin="\begin{itemize}\item{}" end="\end{itemize}"/>
|
|
||||||
<gridsize name="4 pts" value="4"/>
|
|
||||||
<gridsize name="8 pts (~3 mm)" value="8"/>
|
|
||||||
<gridsize name="16 pts (~6 mm)" value="16"/>
|
|
||||||
<gridsize name="32 pts (~12 mm)" value="32"/>
|
|
||||||
<gridsize name="10 pts (~3.5 mm)" value="10"/>
|
|
||||||
<gridsize name="20 pts (~7 mm)" value="20"/>
|
|
||||||
<gridsize name="14 pts (~5 mm)" value="14"/>
|
|
||||||
<gridsize name="28 pts (~10 mm)" value="28"/>
|
|
||||||
<gridsize name="56 pts (~20 mm)" value="56"/>
|
|
||||||
<anglesize name="90 deg" value="90"/>
|
|
||||||
<anglesize name="60 deg" value="60"/>
|
|
||||||
<anglesize name="45 deg" value="45"/>
|
|
||||||
<anglesize name="30 deg" value="30"/>
|
|
||||||
<anglesize name="22.5 deg" value="22.5"/>
|
|
||||||
<tiling name="falling" angle="-60" step="4" width="1"/>
|
|
||||||
<tiling name="rising" angle="30" step="4" width="1"/>
|
|
||||||
</ipestyle>
|
|
||||||
<page>
|
|
||||||
<layer name="alpha"/>
|
|
||||||
<layer name="beta"/>
|
|
||||||
<view layers="alpha beta" active="alpha"/>
|
|
||||||
<text layer="alpha" matrix="1 0 0 1 42.5463 55.5916" transformations="translations" pos="185.118 575.11" stroke="darkgreen" type="label" width="11.68" height="4.293" depth="2.21" valign="baseline">$_w\mathtt{p}$</text>
|
|
||||||
<text matrix="1 0 0 1 37.0859 58.3179" transformations="translations" pos="64 560" stroke="black" type="label" width="22.245" height="14.072" depth="2.15" valign="baseline" size="Large">$\overrightarrow{\mathcal{F}}_w$</text>
|
|
||||||
<path matrix="1 0 0 1 64.3979 50.4555" stroke="black" pen="heavier" arrow="normal/small">
|
|
||||||
64 576 m
|
|
||||||
96 576 l
|
|
||||||
</path>
|
|
||||||
<path matrix="1 0 0 1 64.3979 50.4555" stroke="black" pen="heavier" arrow="normal/small">
|
|
||||||
64 576 m
|
|
||||||
64 608 l
|
|
||||||
</path>
|
|
||||||
<path matrix="1 0 0 1 64.3979 50.4555" stroke="black" pen="heavier" arrow="normal/small">
|
|
||||||
64 576 m
|
|
||||||
80 592 l
|
|
||||||
</path>
|
|
||||||
<path matrix="-0.999969 -0.00790914 0.00790914 -0.999969 456.342 1448.77" stroke="black" pen="heavier" rarrow="normal/small">
|
|
||||||
144 688 m
|
|
||||||
176 688 l
|
|
||||||
</path>
|
|
||||||
<path matrix="-0.999969 -0.00790914 0.00790914 -0.999969 423.99 1448.96" stroke="black" pen="heavier" arrow="normal/small">
|
|
||||||
144 688 m
|
|
||||||
144 720 l
|
|
||||||
</path>
|
|
||||||
<path matrix="-0.999969 -0.00790914 0.00790914 -0.999969 423.99 1448.96" stroke="black" pen="heavier" arrow="normal/small">
|
|
||||||
144 688 m
|
|
||||||
160 704 l
|
|
||||||
</path>
|
|
||||||
<path stroke="red" arrow="normal/small">
|
|
||||||
285.262 759.615 m
|
|
||||||
162.017 735.456
|
|
||||||
129.232 628.801 s
|
|
||||||
</path>
|
|
||||||
<use matrix="1 0 0 1 -17.1996 59.6288" name="mark/disk(sx)" pos="340.469 588.657" size="large" stroke="black"/>
|
|
||||||
<text matrix="1 0 0 1 48.3271 131.44" transformations="translations" pos="124.125 605.151" stroke="red" type="label" width="23.445" height="8.766" depth="4.09" valign="baseline" size="Large">$\mathtt{T}_{f,w}$</text>
|
|
||||||
<path stroke="darkgreen" pen="heavier" arrow="normal/small">
|
|
||||||
128.441 626.745 m
|
|
||||||
320.625 647.224 l
|
|
||||||
</path>
|
|
||||||
<path matrix="1 0 0 1 -16.1315 58.5084" stroke="darkgreen">
|
|
||||||
331.681 587.399 m
|
|
||||||
331.681 587.399 l
|
|
||||||
</path>
|
|
||||||
<text matrix="1 0 0 1 72.9974 12.8488" transformations="translations" pos="238.52 751.212" stroke="black" type="label" width="5.694" height="4.289" depth="0" valign="baseline">$x$</text>
|
|
||||||
<text matrix="1 0 0 1 38.6847 -20.4129" transformations="translations" pos="238.52 751.212" stroke="black" type="label" width="5.071" height="4.289" depth="0" valign="baseline">$z$</text>
|
|
||||||
<text matrix="1 0 0 1 24.5045 -3.95686" transformations="translations" pos="238.52 751.212" stroke="black" type="label" width="5.242" height="4.297" depth="1.93" valign="baseline">$y$</text>
|
|
||||||
<path matrix="1 0 0 1 0 -16" stroke="black">
|
|
||||||
256 736 m
|
|
||||||
224 704 l
|
|
||||||
320 704 l
|
|
||||||
352 736 l
|
|
||||||
h
|
|
||||||
</path>
|
|
||||||
<path stroke="black" arrow="normal/normal">
|
|
||||||
285.262 759.265 m
|
|
||||||
322.176 651.119 l
|
|
||||||
</path>
|
|
||||||
<text matrix="1 0 0 1 -5.60204 -7.00256" transformations="translations" pos="313.272 711.998" stroke="purple" type="label" width="14.765" height="6.175" depth="3.19" valign="baseline" size="Large">$\mathtt{px}$</text>
|
|
||||||
<path stroke="blue" pen="heavier" arrow="normal/normal">
|
|
||||||
285.612 758.915 m
|
|
||||||
315.723 669.632 l
|
|
||||||
</path>
|
|
||||||
<text matrix="1 0 0 1 -11.0998 -37.7176" transformations="translations" pos="313.272 711.998" stroke="blue" type="label" width="7.382" height="8.767" depth="0" valign="baseline" size="Large">$\mathtt{f}$</text>
|
|
||||||
<text matrix="1 0 0 1 210.304 207.851" transformations="translations" pos="64 560" stroke="black" type="label" width="20.795" height="14.069" depth="4.09" valign="baseline" size="Large">$\overrightarrow{\mathcal{F}}_f$</text>
|
|
||||||
<path stroke="purple" pen="heavier" arrow="normal/tiny">
|
|
||||||
255.668 719.918 m
|
|
||||||
274.728 719.957 l
|
|
||||||
</path>
|
|
||||||
<path stroke="purple" pen="heavier" arrow="normal/tiny">
|
|
||||||
256.107 720.028 m
|
|
||||||
246.655 710.527 l
|
|
||||||
</path>
|
|
||||||
<text matrix="1 0 0 1 7.21843 -34.9205" transformations="translations" pos="238.52 751.212" stroke="purple" type="label" width="5.187" height="4.289" depth="0" valign="baseline">$v$</text>
|
|
||||||
<text matrix="1 0 0 1 23.3779 -29.12" transformations="translations" pos="238.52 751.212" stroke="purple" type="label" width="5.703" height="4.289" depth="0" valign="baseline">$u$</text>
|
|
||||||
<use name="mark/disk(sx)" pos="303.819 704.295" size="normal" stroke="purple"/>
|
|
||||||
<text matrix="1 0 0 1 15.6911 -69.9004" transformations="translations" pos="313.272 711.998" stroke="black" type="label" width="8.966" height="6.376" depth="2.79" valign="baseline" size="Large">$\mathbf{p}$</text>
|
|
||||||
<text matrix="1 0 0 1 136.413 88.5801" transformations="translations" pos="185.118 575.11" stroke="black" type="label" width="10.395" height="4.29" depth="2.85" valign="baseline">$_f\mathtt{p}$</text>
|
|
||||||
</page>
|
|
||||||
</ipe>
|
|
||||||
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 27 KiB After Width: | Height: | Size: 30 KiB |
+1026
-1423
File diff suppressed because it is too large
Load Diff
@@ -49,7 +49,6 @@ namespace vio
|
|||||||
const double EPS = 0.0000000001;
|
const double EPS = 0.0000000001;
|
||||||
const double PI = 3.14159265;
|
const double PI = 3.14159265;
|
||||||
|
|
||||||
static boost::mutex feature_detection_mut_;
|
|
||||||
static std::string time_in_HH_MM_SS_MMM()
|
static std::string time_in_HH_MM_SS_MMM()
|
||||||
{
|
{
|
||||||
using namespace std::chrono;
|
using namespace std::chrono;
|
||||||
|
|||||||
@@ -45,10 +45,6 @@ public:
|
|||||||
void stopThread();
|
void stopThread();
|
||||||
|
|
||||||
|
|
||||||
/// If the map is reset, call this function such that we don't have pointers
|
|
||||||
/// to old frames.
|
|
||||||
void reset();
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted.
|
bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted.
|
||||||
boost::thread* thread_;
|
boost::thread* thread_;
|
||||||
|
|||||||
@@ -179,9 +179,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|||||||
new_frame_->Cov_ = init_f.first;
|
new_frame_->Cov_ = init_f.first;
|
||||||
// sparse image align
|
// sparse image align
|
||||||
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
|
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
|
||||||
feature_detection_mut_.lock();
|
|
||||||
size_t err=img_align.run(last_frame_, new_frame_, log_);
|
size_t err=img_align.run(last_frame_, new_frame_, log_);
|
||||||
feature_detection_mut_.unlock();
|
|
||||||
if(err==-1){
|
if(err==-1){
|
||||||
#if VIO_DEBUG
|
#if VIO_DEBUG
|
||||||
fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
|
fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
|
||||||
@@ -211,6 +209,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|||||||
pose_optimizer::optimizeGaussNewton(
|
pose_optimizer::optimizeGaussNewton(
|
||||||
Config::poseOptimThresh(), 70, false,
|
Config::poseOptimThresh(), 70, false,
|
||||||
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
|
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
|
||||||
|
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
|
||||||
#if VIO_DEBUG
|
#if VIO_DEBUG
|
||||||
fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
|
fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
|
||||||
new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
|
new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
|
||||||
@@ -218,7 +217,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|||||||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
|
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
|
||||||
#endif
|
#endif
|
||||||
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
|
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
|
||||||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<5){
|
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<10){
|
||||||
new_frame_=last_frame_;
|
new_frame_=last_frame_;
|
||||||
return RESULT_FAILURE;
|
return RESULT_FAILURE;
|
||||||
}
|
}
|
||||||
@@ -236,7 +235,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|||||||
return RESULT_NO_KEYFRAME;
|
return RESULT_NO_KEYFRAME;
|
||||||
}
|
}
|
||||||
|
|
||||||
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
|
|
||||||
double depth_mean=0.0, depth_min=0.0;
|
double depth_mean=0.0, depth_min=0.0;
|
||||||
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
|
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
|
||||||
new_frame_->setKeyframe();
|
new_frame_->setKeyframe();
|
||||||
@@ -268,7 +266,6 @@ void FrameHandlerMono::resetAll()
|
|||||||
last_frame_.reset();
|
last_frame_.reset();
|
||||||
new_frame_.reset();
|
new_frame_.reset();
|
||||||
overlap_kfs_.clear();
|
overlap_kfs_.clear();
|
||||||
ba_glob_->reset();
|
|
||||||
}
|
}
|
||||||
bool FrameHandlerMono::needNewKf()
|
bool FrameHandlerMono::needNewKf()
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -66,19 +66,6 @@ namespace vio {
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void BA_Glob::reset()
|
|
||||||
{
|
|
||||||
seeds_updating_halt_ = true;
|
|
||||||
{
|
|
||||||
}
|
|
||||||
seeds_updating_halt_ = false;
|
|
||||||
|
|
||||||
#if VIO_DEBUG
|
|
||||||
fprintf(log_,"[%s] DepthFilter: RESET.\n",vio::time_in_HH_MM_SS_MMM().c_str());
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void BA_Glob::updateLoop()
|
void BA_Glob::updateLoop()
|
||||||
{
|
{
|
||||||
while(!boost::this_thread::interruption_requested())
|
while(!boost::this_thread::interruption_requested())
|
||||||
|
|||||||
@@ -132,11 +132,11 @@ void detectFeatures(
|
|||||||
Features& new_features,
|
Features& new_features,
|
||||||
opencl* gpu_fast)
|
opencl* gpu_fast)
|
||||||
{
|
{
|
||||||
feature_detection_mut_.lock();
|
|
||||||
feature_detection::FastDetector detector(
|
feature_detection::FastDetector detector(
|
||||||
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
|
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
|
||||||
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
|
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
|
||||||
feature_detection_mut_.unlock();
|
|
||||||
// now for all maximum corners, initialize a new seed
|
// now for all maximum corners, initialize a new seed
|
||||||
px_vec.clear();
|
px_vec.clear();
|
||||||
for(auto&& ftr:new_features){
|
for(auto&& ftr:new_features){
|
||||||
|
|||||||
@@ -59,7 +59,7 @@ Point::~Point()
|
|||||||
|
|
||||||
void Point::addFrameRef(std::shared_ptr<Feature> ftr)
|
void Point::addFrameRef(std::shared_ptr<Feature> ftr)
|
||||||
{
|
{
|
||||||
|
boost::unique_lock<boost::mutex> lock(point_mut_);
|
||||||
obs_.push_front(ftr);
|
obs_.push_front(ftr);
|
||||||
++n_obs_;
|
++n_obs_;
|
||||||
}
|
}
|
||||||
@@ -138,6 +138,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, std::shared_ptr<Feature>&
|
|||||||
|
|
||||||
void Point::optimize(const size_t n_iter)
|
void Point::optimize(const size_t n_iter)
|
||||||
{
|
{
|
||||||
|
boost::unique_lock<boost::mutex> lock(point_mut_);
|
||||||
Vector3d old_point = pos_;
|
Vector3d old_point = pos_;
|
||||||
double chi2 = 0.0;
|
double chi2 = 0.0;
|
||||||
Matrix3d A;
|
Matrix3d A;
|
||||||
@@ -167,33 +168,21 @@ void Point::optimize(const size_t n_iter)
|
|||||||
// check if error increased
|
// check if error increased
|
||||||
if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
|
if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
|
||||||
{
|
{
|
||||||
#ifdef POINT_OPTIMIZER_DEBUG
|
|
||||||
cout << "it " << i
|
|
||||||
<< "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
|
|
||||||
#endif
|
|
||||||
pos_ = old_point; // roll-back
|
pos_ = old_point; // roll-back
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// update the model
|
// update the model
|
||||||
Vector3d new_point = pos_ + dp;
|
Vector3d new_point = pos_ + dp;
|
||||||
old_point = pos_;
|
old_point = pos_;
|
||||||
pos_ = new_point;
|
pos_ = new_point;
|
||||||
chi2 = new_chi2;
|
chi2 = new_chi2;
|
||||||
#ifdef POINT_OPTIMIZER_DEBUG
|
|
||||||
cout << "it " << i
|
|
||||||
<< "\t Success \t new_chi2 = " << new_chi2
|
|
||||||
<< "\t norm(b) = " << vk::norm_max(b)
|
|
||||||
<< endl;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// stop when converged
|
// stop when converged
|
||||||
if(vk::norm_max(dp) <= EPS)
|
if(vk::norm_max(dp) <= EPS)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#ifdef POINT_OPTIMIZER_DEBUG
|
|
||||||
cout << endl;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace vio
|
} // namespace vio
|
||||||
|
|||||||
@@ -76,11 +76,9 @@ namespace vio {
|
|||||||
Features keypoints_cur;
|
Features keypoints_cur;
|
||||||
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
|
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
|
||||||
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
|
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
|
||||||
feature_detection_mut_.lock();
|
|
||||||
feature_detection::FastDetector detector(
|
feature_detection::FastDetector detector(
|
||||||
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
|
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
|
||||||
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
|
detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
|
||||||
feature_detection_mut_.unlock();
|
|
||||||
list<pair<FramePtr, double> > close_kfs;
|
list<pair<FramePtr, double> > close_kfs;
|
||||||
map_.getCloseKeyframes(frame, close_kfs);
|
map_.getCloseKeyframes(frame, close_kfs);
|
||||||
if (!last_frame->fts_.empty())
|
if (!last_frame->fts_.empty())
|
||||||
|
|||||||
Reference in New Issue
Block a user