correct frame
This commit is contained in:
gfuse
Generated
+8
@@ -0,0 +1,8 @@
|
||||
# Default ignored files
|
||||
/shelf/
|
||||
/workspace.xml
|
||||
# Datasource local storage ignored files
|
||||
/dataSources/
|
||||
/dataSources.local.xml
|
||||
# Editor-based HTTP Client requests
|
||||
/httpRequests/
|
||||
Generated
+6
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$/GPU_version/svo">
|
||||
<contentRoot DIR="$PROJECT_DIR$" />
|
||||
</component>
|
||||
</project>
|
||||
Generated
+8
@@ -0,0 +1,8 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="ProjectModuleManager">
|
||||
<modules>
|
||||
<module fileurl="file://$PROJECT_DIR$/.idea/rpg_svo.iml" filepath="$PROJECT_DIR$/.idea/rpg_svo.iml" />
|
||||
</modules>
|
||||
</component>
|
||||
</project>
|
||||
Generated
+2
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module classpath="CMake" type="CPP_MODULE" version="4" />
|
||||
Generated
+6
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="VcsDirectoryMappings">
|
||||
<mapping directory="$PROJECT_DIR$" vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
@@ -0,0 +1,312 @@
|
||||
<?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.
|
After Width: | Height: | Size: 27 KiB |
@@ -16,7 +16,9 @@
|
||||
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
|
||||
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
|
||||
valuesPtr=std::make_shared<gtsam::Values>();
|
||||
parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
|
||||
gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
|
||||
parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
|
||||
parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
|
||||
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
|
||||
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
|
||||
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
|
||||
@@ -148,4 +150,4 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
Generated
+1
@@ -0,0 +1 @@
|
||||
gpu_svo
|
||||
Generated
+2
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module classpath="CMake" type="CPP_MODULE" version="4" />
|
||||
Generated
+1
@@ -1,6 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="VcsDirectoryMappings">
|
||||
<mapping directory="$PROJECT_DIR$/../.." vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
@@ -16,7 +16,12 @@
|
||||
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
|
||||
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
|
||||
valuesPtr=std::make_shared<gtsam::Values>();
|
||||
parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
|
||||
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
|
||||
// If pitch and roll are zero for an aerospace frame,
|
||||
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
|
||||
gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
|
||||
parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
|
||||
parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
|
||||
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
|
||||
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
|
||||
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
|
||||
@@ -122,14 +127,6 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
|
||||
++num_obs;
|
||||
}
|
||||
// Now optimize.
|
||||
//gtsam::ISAM2 optimizer(*optimizerParamPtr);
|
||||
//optimizer.update(*graphPtr, *valuesPtr);
|
||||
// Each call to iSAM2 update(*) performs one iteration of the iterative
|
||||
// nonlinear solver. If accuracy is desired at the expense of time,
|
||||
// update(*) can be called additional times to perform multiple optimizer
|
||||
// iterations every step.
|
||||
//optimizer.update();
|
||||
// optimizer.print();
|
||||
gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr);
|
||||
|
||||
reset(optimizer,new_frame);
|
||||
|
||||
Generated
+1
@@ -0,0 +1 @@
|
||||
vio_gpu
|
||||
Generated
+2
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module classpath="CMake" type="CPP_MODULE" version="4" />
|
||||
Generated
+1
@@ -1,6 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="VcsDirectoryMappings">
|
||||
<mapping directory="$PROJECT_DIR$/../.." vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
Reference in New Issue
Block a user