From bb2cf89284635130f526d87683cdf4aecfd029a9 Mon Sep 17 00:00:00 2001 From: AndreasJaeger Date: Tue, 2 Jun 2015 15:17:24 +0200 Subject: [PATCH] Updated doxygen documentation --- include/rovio/CameraOutputCF.hpp | 2 +- include/rovio/FilterStates.hpp | 4 +- include/rovio/ImgUpdate.hpp | 166 +++++++++++++++++++++++++++---- include/rovio/RovioNode.hpp | 70 ++++++++++--- include/rovio/rovioFilter.hpp | 18 ++++ src/test_rovio.cpp | 2 +- 6 files changed, 221 insertions(+), 41 deletions(-) diff --git a/include/rovio/CameraOutputCF.hpp b/include/rovio/CameraOutputCF.hpp index 3314759a..31927848 100644 --- a/include/rovio/CameraOutputCF.hpp +++ b/include/rovio/CameraOutputCF.hpp @@ -61,9 +61,9 @@ class CameraOutputCF:public LWF::CoordinateTransform{ ~CameraOutputCF(){}; void eval(mtOutput& output, const mtInput& input, const mtMeas& meas, double dt = 0.0) const{ // IrIV = IrIM + qIM*(MrMV) + // VwIV = qVM*MwIM // VvV = qVM*(MvM + MwIM x MrMV) // qVI = qVM*qIM^T - // VwIV = qVM*MwIM output.template get() = input.template get()+input.template get().rotate(input.get_MrMV(camID_)); output.template get() = input.get_qVM(camID_).rotate(V3D(input.template get().MwIMmeas_-input.template get())); output.template get() = diff --git a/include/rovio/FilterStates.hpp b/include/rovio/FilterStates.hpp index 0c10dd26..5c7478df 100644 --- a/include/rovio/FilterStates.hpp +++ b/include/rovio/FilterStates.hpp @@ -245,7 +245,7 @@ class StateAuxiliary: public LWF::AuxiliaryBase>{ static constexpr unsigned int nMax_ = nMax; static constexpr unsigned int nLevels_ = nLevels; static constexpr unsigned int patchSize_ = patchSize; - static constexpr unsigned int nCam_ = nCam; + static constexpr unsigned int nCam_ = nCam; /** class ImgInnovation: public LWF::State>{ public: @@ -54,6 +56,11 @@ class ImgInnovation: public LWF::State>{ }; ~ImgInnovation(){}; }; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/** \brief Class, holding the image pyramids of the different cameras. @todo complete + */ template class ImgUpdateMeasAuxiliary: public LWF::AuxiliaryBase>{ public: @@ -77,6 +84,11 @@ class ImgUpdateMeasAuxiliary: public LWF::AuxiliaryBase class ImgUpdateMeas: public LWF::State>{ public: @@ -88,6 +100,11 @@ class ImgUpdateMeas: public LWF::State>{ }; ~ImgUpdateMeas(){}; }; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/** \brief @todo + */ template class ImgUpdateNoise: public LWF::State>{ public: @@ -100,16 +117,26 @@ class ImgUpdateNoise: public LWF::State>{ }; ~ImgUpdateNoise(){}; }; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/** \brief Outlier Detection. + */ template class ImgOutlierDetection: public LWF::OutlierDetection::template getId::_nor>(),2>>{ }; +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/** \brief Class, holding image update routines for the filter. + */ +template template class ImgUpdate: public LWF::Update,FILTERSTATE,ImgUpdateMeas,ImgUpdateNoise, - ImgOutlierDetection,false>{ +ImgOutlierDetection,false>{ public: typedef LWF::Update,FILTERSTATE,ImgUpdateMeas,ImgUpdateNoise, - ImgOutlierDetection,false> Base; + ImgOutlierDetection,false> Base; using typename Base::eval; using Base::doubleRegister_; using Base::intRegister_; @@ -152,11 +179,17 @@ class ImgUpdate: public LWF::Update double matchingPixelThreshold_; double patchRejectionTh_; bool doPatchWarping_; - bool useDirectMethod_; + bool useDirectMethod_; /** doubleRegister_.registerScalar("UpdateNoise.nor",updnoiP_(1,1)); useImprovedJacobian_ = false; // TODO: adapt/test }; + + /** \brief Destructor + */ ~ImgUpdate(){}; + + /** \brief @todo + */ void refreshProperties(){ useSpecialLinearizationPoint_ = true; // TODO: make dependent }; + + /** \brief @todo + */ void setCamera(Camera* mpCameras){ mpCameras_ = mpCameras; pixelOutputCF_.setCamera(mpCameras); pixelOutputFromNorCF_.setCamera(mpCameras); } + + /** \brief Sets the innovation term. + * + * \note If \ref useDirectMethod_ is set true, the innovation term is based directly on pixel intensity errors. + * \note If \ref useDirectMethod_ is set false, the reprojection error is used for the innovation term. + * @param mtInnovation - Class, holding innovation data. + * @param state - Filter %State. + * @param meas - Not Used. + * @param noise - Additive discrete Gaussian noise. + * @param dt - Not used. + * @todo check this + */ void eval(mtInnovation& y, const mtState& state, const mtMeas& meas, const mtNoise noise, double dt = 0.0) const{ - const int& ID = state.template get().activeFeature_; - const int& camID = state.template get().camID_[ID]; + const int& ID = state.template get().activeFeature_; // Feature ID + const int& camID = state.template get().camID_[ID]; // Camera ID of the feature. const int& activeCamCounter = state.template get().activeCameraCounter_; const int activeCamID = (activeCamCounter + camID)%mtState::nCam_; if(verbose_){ @@ -241,6 +295,17 @@ class ImgUpdate: public LWF::Update y.template get() += noise.template get(); } } + + /** \brief Computes the Jacobian for the update step of the filter. + * + * \note If \ref useDirectMethod_ is set true, the jacobian is set w.r.t. the intensity errors. + * \note If \ref useDirectMethod_ is set false, the jacobian is set w.r.t. the reprojection error. + * @param F - Jacobian for the update step of the filter. + * @param state - Filter state. + * @param meas - Not used. + * @param dt - Not used. + * @todo check this + */ void jacInput(mtJacInput& F, const mtState& state, const mtMeas& meas, double dt = 0.0) const{ const int& ID = state.template get().activeFeature_; const int& camID = state.template get().camID_[ID]; @@ -258,14 +323,25 @@ class ImgUpdate: public LWF::Update F = -state.template get().A_red_[ID]*c_J*featureLocationOutputJac_.template block<2,mtState::D_>(0,0); } else { F = state.template get().bearingMeas_[ID].getN().transpose() - *-LWF::NormalVectorElement::getRotationFromTwoNormalsJac(featureLocationOutput_.template get(),state.template get().bearingMeas_[ID]) - *featureLocationOutput_.template get().getM()*featureLocationOutputJac_.template block<2,mtState::D_>(0,0); + *-LWF::NormalVectorElement::getRotationFromTwoNormalsJac(featureLocationOutput_.template get(),state.template get().bearingMeas_[ID]) + *featureLocationOutput_.template get().getM()*featureLocationOutputJac_.template block<2,mtState::D_>(0,0); } } + + /** \brief @todo + */ void jacNoise(mtJacNoise& G, const mtState& state, const mtMeas& meas, double dt = 0.0) const{ G.setZero(); G.template block<2,2>(mtInnovation::template getId(),mtNoise::template getId()) = Eigen::Matrix2d::Identity(); } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Prepares the filter state for the update. + * + * @param filterState - Filter state. + * @param meas - Update measurement. + */ void commonPreProcess(mtFilterState& filterState, const mtMeas& meas){ assert(filterState.t_ == meas.template get().imgTime_); for(int i=0;i // TODO sort feature by covariance and use more accurate ones first } + + /** \brief Pre-Processing for the image update. + * + * Summary: + * 1. Searches a valid MultilevelPatchFeature from the filter state. + * 2. Transforms the MultilevelPatchFeature into the target frame. + * 3. Executes a 2D patch alignment in the target frame. If unsuccessful go back to step 1. + * 4. If bearing error between aligned patch and estimated patch too large, alter linearization point. + * Bearing vector in the state is directly altered to the new aligned position. + * + * @param filterState - Filter state. + * @param meas - Update measurement. + * @param isFinished - True, if process has finished. + * @todo check and complete. + */ void preProcess(mtFilterState& filterState, const mtMeas& meas, bool& isFinished){ if(isFinished){ // gets called if this is the first call commonPreProcess(filterState,meas); @@ -292,7 +383,7 @@ class ImgUpdate: public LWF::Update Eigen::Vector2d bearingError; typename mtFilterState::mtState& state = filterState.state_; typename mtFilterState::mtFilterCovMat& cov = filterState.cov_; - int& ID = filterState.state_.template get().activeFeature_; + int& ID = filterState.state_.template get().activeFeature_; // ID of the current updated feature!!! Initially set to 0. int& activeCamCounter = filterState.state_.template get().activeCameraCounter_; // Actualize camera extrinsics @@ -303,10 +394,10 @@ class ImgUpdate: public LWF::Update while(ID < mtState::nMax_ && foundValidMeasurement == false){ if(filterState.mlps_.isValid_[ID]){ // Data handling stuff - mpFeature = &filterState.mlps_.features_[ID]; - mpFeature->fromState(); - mpFeature->setDepth(state.get_depth(ID)); - const int camID = mpFeature->camID_; + mpFeature = &filterState.mlps_.features_[ID]; // Set the feature pointer. + mpFeature->fromState(); // Read the linked variable from the filter state. + mpFeature->setDepth(state.get_depth(ID)); // Depth value. + const int camID = mpFeature->camID_; // Camera ID of the feature. if(activeCamCounter==0){ mpFeature->increaseStatistics(filterState.t_); if(verbose_){ @@ -403,7 +494,7 @@ class ImgUpdate: public LWF::Update if(verbose_) std::cout << " Backprojection: " << linearizationPoint.template get(ID).getVec().transpose() << std::endl; if(useDirectMethod_ && !useSpecialLinearizationPoint_) patchInTargetFrame.set_nor(featureLocationOutput_.template get()); if(!useDirectMethod_ || getLinearAlignEquationsReduced(patchInTargetFrame,meas.template get().pyr_[activeCamID],endLevel_,startLevel_,doPatchWarping_, - state.template get().A_red_[ID],state.template get().b_red_[ID])){ + state.template get().A_red_[ID],state.template get().b_red_[ID])){ if(useSpecialLinearizationPoint_) linearizationPoint.boxMinus(state,filterState.difVecLin_); state.template get().bearingMeas_[ID] = patchInTargetFrame.get_nor(); foundValidMeasurement = true; @@ -435,13 +526,26 @@ class ImgUpdate: public LWF::Update ID++; } } - } + } // while end if(ID >= mtState::nMax_){ isFinished = true; } }; + + /** \brief Post-Processing for the image update. + * + * Summary: + * 1. Some drawing tasks. + * 2. Removal of features with negative depth. + * + * @param filterState - Filter state. + * @param meas - Update measurement. + * @param outlierDetection - Outlier detection. + * @param isFinished - True, if process has finished. + * @todo check and complete. + */ void postProcess(mtFilterState& filterState, const mtMeas& meas, const mtOutlierDetection& outlierDetection, bool& isFinished){ - int& ID = filterState.state_.template get().activeFeature_; + int& ID = filterState.state_.template get().activeFeature_; // Get the ID of the updated feature. int& activeCamCounter = filterState.state_.template get().activeCameraCounter_; if(isFinished){ @@ -475,8 +579,8 @@ class ImgUpdate: public LWF::Update if(filterState.mlps_.isValid_[i]){ if(filterState.state_.template get(i) < 1e-8){ if(verbose_) std::cout << " \033[33mRemoved feature " << i << " with invalid depth " << filterState.state_.get_depth(i) << "!\033[0m" << std::endl; - filterState.mlps_.isValid_[i] = false; - filterState.removeFeature(i); + filterState.mlps_.isValid_[i] = false; + filterState.removeFeature(i); } } } @@ -489,6 +593,18 @@ class ImgUpdate: public LWF::Update } } }; + + /** \brief Final Post-Processing step for the image update. + * + * Summary: + * 1. For each feature in the state: Extract patches and compute Shi-Tomasi Score. + * 2. Removal of bad features from the state. + * 3. Get new features and add them to the state. + * + * @param filterState - Filter state. + * @param meas - Update measurement. + * @todo check and complete. + */ void commonPostProcess(mtFilterState& filterState, const mtMeas& meas){ // Temps float averageScore; @@ -507,10 +623,11 @@ class ImgUpdate: public LWF::Update } countTracked = 0; + // For all features in the state. for(unsigned int i=0;ifromState(); // Has to be done for every valid feature + mpFeature->fromState(); // Has to be done for every valid feature. mpFeature->setDepth(state.get_depth(i)); const int camID = mpFeature->camID_; if(mpFeature->status_.inFrame_){ @@ -525,7 +642,7 @@ class ImgUpdate: public LWF::Update countTracked++; } - // Extract feature patches + // Extract feature patches and update Shi-Tomasi score. if(mpFeature->status_.trackingStatus_ == TRACKED){ if(isMultilevelPatchInFrame(*mpFeature,meas.template get().pyr_[camID],startLevel_,true,false)){ testFeature.set_c(mpFeature->get_c()); @@ -542,7 +659,7 @@ class ImgUpdate: public LWF::Update } } - // Remove bad feature + // Remove bad feature. averageScore = filterState.mlps_.getAverageScore(); // TODO improve for(unsigned int i=0;i if(filterState.mlps_.isValid_[featureIndex]){ mpFeature = &filterState.mlps_.features_[featureIndex]; if(mpFeature->status_.trackingStatus_ != TRACKED && - !mpFeature->isGoodFeature(trackingLocalRange_,trackingLocalVisibilityRange_,trackingUpperBound_*removalFactor,trackingLowerBound_*removalFactor)){ // TODO: improve + !mpFeature->isGoodFeature(trackingLocalRange_,trackingLocalVisibilityRange_,trackingUpperBound_*removalFactor,trackingLowerBound_*removalFactor)){ // TODO: improve filterState.mlps_.isValid_[featureIndex] = false; filterState.removeFeature(featureIndex); } @@ -624,6 +741,11 @@ class ImgUpdate: public LWF::Update } } } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief @todo + */ void drawVirtualHorizon(mtFilterState& filterState, const int camID = 0){ typename mtFilterState::mtState& state = filterState.state_; cv::rectangle(filterState.img_[camID],cv::Point2f(0,0),cv::Point2f(82,92),cv::Scalar(50,50,50),-1,8,0); diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 496df281..ec9e6daa 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -59,8 +59,8 @@ class RovioNode{ ros::Publisher pubOdometry_; ros::Publisher pubTransform_; tf::TransformBroadcaster tb_; - ros::Publisher pubPcl_; - ros::Publisher pubURays_; + ros::Publisher pubPcl_; /**("/rovio/pcl", 1); pubURays_ = nh_.advertise("/rovio/urays", 1 ); - - // // p21012_equidist_190215 // Eigen::Matrix3d R_VM; // R_VM << 0.9999327192366118, -0.0044421301653885, 0.010715618492127002, @@ -129,11 +129,17 @@ class RovioNode{ poseMsgSeq_ = 1; isInitialized_ = false; } + + /** \brief Destructor + */ ~RovioNode(){} + + /** \brief Tests the functionality of the rovio node. + */ void makeTest(){ mtState testState = mpFilter_->init_.state_; unsigned int s = 2; - testState.setRandom(s); // TODO: debug with doVECalibration = false and depthType = 0 + testState.setRandom(s); // TODO: debug with doVECalibration = false and depthType = 0 predictionMeas_.setRandom(s); imgUpdateMeas_.setRandom(s); @@ -172,6 +178,9 @@ class RovioNode{ featureLocationOutputCFTest.setOutputCameraID(0); featureLocationOutputCFTest.testJacInput(1e-8,1e-5,s,0.1); } + + /** \brief Callback for IMU-Messages. Adds IMU measurements (as prediction measurements) to the filter. + */ void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg){ predictionMeas_.template get() = Eigen::Vector3d(imu_msg->linear_acceleration.x,imu_msg->linear_acceleration.y,imu_msg->linear_acceleration.z); predictionMeas_.template get() = Eigen::Vector3d(imu_msg->angular_velocity.x,imu_msg->angular_velocity.y,imu_msg->angular_velocity.z); @@ -184,12 +193,28 @@ class RovioNode{ isInitialized_ = true; } } + + /** \brief Image callback for the camera with ID 0 + * + * @param img - Image message. + */ void imgCallback0(const sensor_msgs::ImageConstPtr & img){ imgCallback(img,0); } + + /** \brief Image callback for the camera with ID 1 + * + * @param img - Image message. + */ void imgCallback1(const sensor_msgs::ImageConstPtr & img){ if(mtState::nCam_ > 1) imgCallback(img,1); //TODO generalize } + + /** \brief Image callback. Adds images (as update measurements) to the filter. + * + * @param img - Image message. + * @param camID - Camera ID. + */ void imgCallback(const sensor_msgs::ImageConstPtr & img, const int camID = 0){ // Get image from msg cv_bridge::CvImagePtr cv_ptr; @@ -224,13 +249,20 @@ class RovioNode{ } } } + + /** \brief Executes the update step of the filter and publishes updated data. @todo check this + * + * @param updateTime - Update Time. + */ void updateAndPublish(const double& updateTime){ if(isInitialized_){ + + // Execute the filter update. const double t1 = (double) cv::getTickCount(); int c1 = std::get<0>(mpFilter_->updateTimelineTuple_).measMap_.size(); static double timing_T = 0; static int timing_C = 0; - mpFilter_->updateSafe(&updateTime); // TODO: continue inly if actually updates + mpFilter_->updateSafe(&updateTime); // TODO: continue only if actually updates const double t2 = (double) cv::getTickCount(); int c2 = std::get<0>(mpFilter_->updateTimelineTuple_).measMap_.size(); timing_T += (t2-t1)/cv::getTickFrequency()*1000; @@ -245,15 +277,19 @@ class RovioNode{ cv::waitKey(5); } } + + // Obtain the save filter state. mtFilterState& filterState = mpFilter_->safe_; mtState& state = mpFilter_->safe_.state_; typename mtFilterState::mtFilterCovMat& cov = mpFilter_->safe_.cov_; cameraOutputCF_.transformState(state,output_); cameraOutputCF_.transformCovMat(state,cov,outputCov_); + // Get the position and orientation. Eigen::Vector3d IrIV = output_.template get(); rot::RotationQuaternionPD qVI = output_.template get(); + // Send camera pose message. poseMsg_.header.seq = poseMsgSeq_; poseMsg_.header.stamp = ros::Time(mpFilter_->safe_.t_); poseMsg_.pose.position.x = IrIV(0); @@ -265,6 +301,7 @@ class RovioNode{ poseMsg_.pose.orientation.z = qVI.z(); pubPose_.publish(poseMsg_); + // Send camera pose. tf::StampedTransform tf_transform_v; tf_transform_v.frame_id_ = "world"; tf_transform_v.child_frame_id_ = "camera"; @@ -273,6 +310,7 @@ class RovioNode{ tf_transform_v.setRotation(tf::Quaternion(qVI.x(),qVI.y(),qVI.z(),qVI.w())); tb_.sendTransform(tf_transform_v); + // Send IMU pose. tf::StampedTransform tf_transform; tf_transform.frame_id_ = "world"; tf_transform.child_frame_id_ = "imu"; @@ -283,6 +321,7 @@ class RovioNode{ tf_transform.setRotation(tf::Quaternion(qMI.x(),qMI.y(),qMI.z(),qMI.w())); tb_.sendTransform(tf_transform); + // Send IMU pose message. transformMsg_.header = poseMsg_.header; transformMsg_.transform.translation.x = IrIM(0); transformMsg_.transform.translation.y = IrIM(1); @@ -293,16 +332,13 @@ class RovioNode{ transformMsg_.transform.rotation.w = qMI.w(); pubTransform_.publish(transformMsg_); - rovioOutputMsg_.header.seq = poseMsgSeq_; - rovioOutputMsg_.header.stamp = ros::Time(mpFilter_->safe_.t_); + // Odometry odometryMsg_.header.seq = poseMsgSeq_; odometryMsg_.header.stamp = ros::Time(mpFilter_->safe_.t_); - - // Odometry - odometryMsg_.pose.pose.position.x = output_.template get()(0); + odometryMsg_.pose.pose.position.x = output_.template get()(0); // IrIV odometryMsg_.pose.pose.position.y = output_.template get()(1); odometryMsg_.pose.pose.position.z = output_.template get()(2); - odometryMsg_.pose.pose.orientation.w = output_.template get().w(); + odometryMsg_.pose.pose.orientation.w = output_.template get().w(); // qVI odometryMsg_.pose.pose.orientation.x = output_.template get().x(); odometryMsg_.pose.pose.orientation.y = output_.template get().y(); odometryMsg_.pose.pose.orientation.z = output_.template get().z(); @@ -315,10 +351,10 @@ class RovioNode{ odometryMsg_.pose.covariance[j+6*i] = outputCov_(ind1,ind2); } } - odometryMsg_.twist.twist.linear.x = output_.template get()(0); + odometryMsg_.twist.twist.linear.x = output_.template get()(0); // VvV odometryMsg_.twist.twist.linear.y = output_.template get()(1); odometryMsg_.twist.twist.linear.z = output_.template get()(2); - odometryMsg_.twist.twist.angular.x = output_.template get()(0); + odometryMsg_.twist.twist.angular.x = output_.template get()(0); // VwIV odometryMsg_.twist.twist.angular.y = output_.template get()(1); odometryMsg_.twist.twist.angular.z = output_.template get()(2); for(unsigned int i=0;i<6;i++){ @@ -330,11 +366,15 @@ class RovioNode{ odometryMsg_.twist.covariance[j+6*i] = outputCov_(ind1,ind2); } } - rovioOutputMsg_.odometry = odometryMsg_; + attitudeOutput_.template get() = output_.template get(); attitudeOutputCov_ = outputCov_.template block<3,3>(mtOutput::template getId(),mtOutput::template getId()); attitudeToYprCF_.transformState(attitudeOutput_,yprOutput_); attitudeToYprCF_.transformCovMat(attitudeOutput_,attitudeOutputCov_,yprOutputCov_); + + rovioOutputMsg_.header.seq = poseMsgSeq_; + rovioOutputMsg_.header.stamp = ros::Time(mpFilter_->safe_.t_); + rovioOutputMsg_.odometry = odometryMsg_; rovioOutputMsg_.ypr_odometry.x = yprOutput_.template get()(0); rovioOutputMsg_.ypr_odometry.y = yprOutput_.template get()(1); rovioOutputMsg_.ypr_odometry.z = yprOutput_.template get()(2); diff --git a/include/rovio/rovioFilter.hpp b/include/rovio/rovioFilter.hpp index 9de07796..3cf3b98e 100644 --- a/include/rovio/rovioFilter.hpp +++ b/include/rovio/rovioFilter.hpp @@ -63,6 +63,9 @@ class RovioFilter:public LWF::FilterBase,ImgUpdate(mUpdates_).setCamera(cameras_); // TODO IMG boolRegister_.registerScalar("Common.doVECalibration",init_.state_.template get().doVECalibration_); @@ -101,21 +104,36 @@ class RovioFilter:public LWF::FilterBase,ImgUpdate(mUpdates_).verbose_); reset(0.0); } + + /** \brief @todo + */ void refreshProperties(){ for(int camID = 0;camID().depthMap_.setType(init_.state_.template get().depthTypeInt_); }; + + /** \brief Destructor + */ ~RovioFilter(){}; // void resetToImuPose(V3D IrIM, QPD qMI, double t = 0.0){ // init_.state_.initWithImuPose(IrIM,qMI); // reset(t); // } + + /** \brief Resets the filter with an accelerometer measurement. + * + * @param fMeasInit - Accelerometer measurement. + * @param t - Current time. @todo check this + */ void resetWithAccelerometer(const V3D& fMeasInit, double t = 0.0){ init_.initWithAccelerometer(fMeasInit); reset(t); } + + /** \brief Sets the transformation between IMU and Camera. + */ void setExtrinsics(const Eigen::Matrix3d& R_VM, const Eigen::Vector3d& VrVM, const int camID = 0){ rot::RotationMatrixAD R(R_VM); init_.state_.template get().qVM_[camID] = QPD(R.getPassive()); diff --git a/src/test_rovio.cpp b/src/test_rovio.cpp index c0f8aa2d..db65d0e4 100644 --- a/src/test_rovio.cpp +++ b/src/test_rovio.cpp @@ -37,7 +37,7 @@ static constexpr unsigned int nMax_ = 25; static constexpr int nLevels_ = 4; static constexpr int patchSize_ = 8; -static constexpr int nCam_ = 2; +static constexpr int nCam_ = 1; typedef rovio::RovioFilter> mtFilter; #ifdef MAKE_SCENE