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

debug GPU version

Browse Source
This commit is contained in:
gfuse
2021-05-26 10:50:20 +02:00
parent 1fba26dfef
commit cd6f0c5b97
58 changed files with 3828 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/svo/include/gpu_svo/svoVisionFactor.h
+15 -12
View File
@@ -16,13 +16,11 @@
namespace svo {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
class VisionFactor: public gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3> {
private:
// measurement information
gtsam::Point3 feature; ///<Unit-bearing vector of the feature.
gtsam::Point3 landmark; ///<Unit-bearing vector of the feature.
boost::optional<gtsam::Point3> body_P_sensor_; ///< TODO The pose of the sensor in the body frame
int level; ///< Image pyramid level where feature was extracted.
public:
@@ -39,22 +37,27 @@ namespace svo {
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param level Image pyramid level where feature was extracted.
*/
VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp,
gtsam::Key poseKey,boost::optional<gtsam::Point3> body_P_sensor = boost::none) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){}
VisionFactor(const gtsam::Point3& feature,const int level,
const gtsam::SharedNoiseModel& H,
gtsam::Key poseKey,gtsam::Key landKey ) :
gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3>(H, poseKey,landKey), feature(feature),level(level){}
virtual ~VisionFactor(){}
/// error function
/// @param p the Estimated pose in Pose3
/// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> Hp = boost::none) const {
gtsam::Vector evaluateError(const gtsam::Pose3& p,const gtsam::Point3& l,
boost::optional<gtsam::Matrix&> H1 = boost::none,boost::optional<gtsam::Matrix&> H2 = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
gtsam::Matrix26 J;
gtsam::Point3 point(p*landmark);
svo::Frame::jacobian_xyz2uv(point, J);
Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
gtsam::Matrix26 J1;
gtsam::Matrix23 J2;
gtsam::Point3 point(p*l);
svo::Frame::jacobian_xyz2uv(point, J1);
svo::Frame::jacobian_l2uv(point, J2);
Vector2d e=(vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
if(H1)(*H1) = (gtsam::Matrix(2,6)<< J1*1.0 / (1<<level)).finished();
if(H2)(*H2) = (gtsam::Matrix(2,3)<< J2*1.0 / (1<<level)).finished();
return e;
}
// The second is a 'clone' function that allows the factor to be copied. Under most