void jacTransform(MXD& J, const mtInput& input) const{ J.setZero(); const int& camID = input.CfP(ID_).camID_; if(camID != outputCamID_){ input.updateMultiCameraExtrinsics(mpMultiCamera_); const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID))); const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD)); const double d_out = DrDP.norm(); const LWF::NormalVectorElement nor_out(DrDP); // TODO: test if Jacobian works, this new setting of vector is dangerous const Eigen::Matrix<double,1,3> J_d_DrDP = nor_out.getVec().transpose(); const Eigen::Matrix<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out; const Eigen::Matrix<double,3,3> J_DrDP_qDC = gSM(DrDP); const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix(); const Eigen::Matrix<double,3,3> J_DrDP_CrCD = -MPD(qDC).matrix(); const Eigen::Matrix<double,3,3> J_qDC_qDB = Eigen::Matrix3d::Identity(); const Eigen::Matrix<double,3,3> J_qDC_qCB = -MPD(qDC).matrix(); const Eigen::Matrix<double,3,3> J_CrCD_qCB = gSM(CrCD); const Eigen::Matrix<double,3,3> J_CrCD_BrBC = -MPD(input.qCM(camID)).matrix(); const Eigen::Matrix<double,3,3> J_CrCD_BrBD = MPD(input.qCM(camID)).matrix(); const Eigen::Matrix<double,3,1> J_CrCP_d = input.CfP(ID_).get_nor().getVec()*input.dep(ID_).getDistanceDerivative(); const Eigen::Matrix<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); J.template block<2,2>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor; J.template block<2,1>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)+2) = J_nor_DrDP*J_DrDP_CrCP*J_CrCP_d; if(!ignoreDistanceOutput_){ J.template block<1,2>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_nor; J.template block<1,1>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)+2) = J_d_DrDP*J_DrDP_CrCP*J_CrCP_d; } if(input.aux().doVECalibration_ && camID != outputCamID_){ J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vea>(camID)) = J_nor_DrDP*(J_DrDP_qDC*J_qDC_qCB+J_DrDP_CrCD*J_CrCD_qCB); J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vea>(outputCamID_)) = J_nor_DrDP*J_DrDP_qDC*J_qDC_qDB; J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vep>(camID)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBC; J.template block<2,3>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_vep>(outputCamID_)) = J_nor_DrDP*J_DrDP_CrCD*J_CrCD_BrBD; if(!ignoreDistanceOutput_){ J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vea>(camID)) = J_d_DrDP*(J_DrDP_qDC*J_qDC_qCB+J_DrDP_CrCD*J_CrCD_qCB); J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vea>(outputCamID_)) = J_d_DrDP*J_DrDP_qDC*J_qDC_qDB; J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vep>(camID)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBC; J.template block<1,3>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_vep>(outputCamID_)) = J_d_DrDP*J_DrDP_CrCD*J_CrCD_BrBD; } } } else { J.template block<2,2>(mtOutput::template getId<mtOutput::_fea>(),mtInput::template getId<mtInput::_fea>(ID_)) = Eigen::Matrix2d::Identity(); J.template block<1,1>(mtOutput::template getId<mtOutput::_fea>()+2,mtInput::template getId<mtInput::_fea>(ID_)+2) = Eigen::Matrix<double,1,1>::Identity(); } }
void evalTransform(mtOutput& output, const mtInput& input) const{ input.updateMultiCameraExtrinsics(mpMultiCamera_); mpMultiCamera_->transformFeature(outputCamID_,input.CfP(ID_),input.dep(ID_),output.c(),output.d()); if(input.CfP(ID_).trackWarping_ && input.CfP(ID_).com_warp_nor()){ const int& camID = input.CfP(ID_).camID_; const QPD qDC = input.qCM(outputCamID_)*input.qCM(camID).inverted(); // TODO: avoid double computation const V3D CrCD = input.qCM(camID).rotate(V3D(input.MrMC(outputCamID_)-input.MrMC(camID))); const V3D CrCP = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getVec(); const V3D DrDP = qDC.rotate(V3D(CrCP-CrCD)); const double d_out = DrDP.norm(); const LWF::NormalVectorElement nor_out(DrDP); const Eigen::Matrix<double,2,3> J_nor_DrDP = nor_out.getM().transpose()/d_out; const Eigen::Matrix<double,3,3> J_DrDP_CrCP = MPD(qDC).matrix(); const Eigen::Matrix<double,3,2> J_CrCP_nor = input.dep(ID_).getDistance()*input.CfP(ID_).get_nor().getM(); output.c().set_warp_nor(J_nor_DrDP*J_DrDP_CrCP*J_CrCP_nor*input.CfP(ID_).get_warp_nor()); } }
bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d){ const V3D C2v1 = qC2C1.rotate(get_nor().getVec()); const V3D C2v2 = other.get_nor().getVec(); const double a = 1.0-pow(C2v1.dot(C2v2),2.0); if(a < 1e-3){ return false; } const double distance = C2v1.dot((M3D::Identity()-C2v2*C2v2.transpose())*C2rC2C1) / a; d.setParameter(fabs(distance)); return true; }
bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d) { Eigen::Matrix<double,3,2> B; B << qC2C1.rotate(get_nor().getVec()), other.get_nor().getVec(); const Eigen::Matrix2d BtB = B.transpose() * B; if(BtB.determinant() < 0.000001) { // Projection rays almost parallel. return false; } const Eigen::Vector2d dv = - BtB.inverse() * B.transpose() * C2rC2C1; d.setParameter(fabs(dv[0])); return true; }
/** \brief Initializes the FilterState \ref Base::state_ with the IMU-Pose. * * @param WrWM - Position Vector, pointing from the World-Frame to the IMU-Frame, expressed in World-Coordinates. * @param qMW - Quaternion, expressing World-Frame in IMU-Coordinates (World Coordinates->IMU Coordinates) */ void initWithImuPose(V3D WrWM, QPD qMW){ state_.WrWM() = WrWM; state_.qWM() = qMW.inverted(); }