65 lines
2.5 KiB
C++
65 lines
2.5 KiB
C++
//
|
|
// Created by root on 4/13/21.
|
|
//
|
|
|
|
#ifndef SVO_SVOVISIONFACTOR_H
|
|
#define SVO_SVOVISIONFACTOR_H
|
|
#pragma once
|
|
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/base/Vector.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <vikit/robust_cost.h>
|
|
#include <vikit/math_utils.h>
|
|
#include <gpu_svo/frame.h>
|
|
|
|
namespace svo {
|
|
|
|
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
|
|
|
|
private:
|
|
// measurement information
|
|
gtsam::Pose3 pose; ///<Estimated pose in svo after GaussNewton optimization.
|
|
|
|
public:
|
|
/// shorthand for a smart pointer to a factor
|
|
typedef boost::shared_ptr<VisionFactor> shared_ptr;
|
|
|
|
/**
|
|
* Constructor
|
|
* @param pose Camera pose.
|
|
* @param pointKey is the index of the landmark
|
|
*/
|
|
VisionFactor(const gtsam::Pose3& pose,
|
|
const gtsam::SharedNoiseModel& H,
|
|
gtsam::Key poseKey) :
|
|
gtsam::NoiseModelFactor1<gtsam::Pose3>(H, poseKey), pose(pose){}
|
|
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&> H1 = boost::none) const {
|
|
// note that use boost optional like a pointer
|
|
// only calculate jacobian matrix when non-null pointer exists
|
|
Eigen::Matrix<double,3,3> E_r=0.5*(p.rotation().matrix().transpose()*pose.rotation().matrix()-pose.rotation().matrix().transpose()*p.rotation().matrix());
|
|
Vector3d e_t=p.translation()-pose.translation();
|
|
if(H1)(*H1) = (gtsam::Matrix(3,6)<< 1,0,0,0,0,0,
|
|
0,0,1,0,0,0,
|
|
0,0,0,0,1,0).finished();
|
|
return (gtsam::Matrix(3,1)<<e_t.x(),e_t.z(),-E_r(0,2)).finished();
|
|
}
|
|
// The second is a 'clone' function that allows the factor to be copied. Under most
|
|
// circumstances, the following code that employs the default copy constructor should
|
|
// work fine.
|
|
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
|
gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); }
|
|
|
|
};
|
|
|
|
} // namespace gtsamexamples
|
|
#endif //SVO_SVOVISIONFACTOR_H
|