//*************************************************************************
TEST (EssentialMatrixFactor3, extraTest) {
// The "true E" in the body frame is
EssentialMatrix bodyE = cRb.inverse() * trueE;
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K);
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
}
}
//*************************************************************************
TEST (EssentialMatrixFactor2, factor) {
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
// Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = PinholeBase::Project(P2);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
}
}
::ceres::ResidualBlockId Estimator::addObservation(uint64_t landmarkId,
uint64_t poseId,
size_t camIdx,
size_t keypointIdx) {
OKVIS_ASSERT_TRUE_DBG(Exception, isLandmarkAdded(landmarkId),
"landmark not added");
// avoid double observations
okvis::KeypointIdentifier kid(poseId, camIdx, keypointIdx);
if (landmarksMap_.at(landmarkId).observations.find(kid)
!= landmarksMap_.at(landmarkId).observations.end()) {
return NULL;
}
// get the keypoint measurement
okvis::MultiFramePtr multiFramePtr = multiFramePtrMap_.at(poseId);
Eigen::Vector2d measurement;
multiFramePtr->getKeypoint(camIdx, keypointIdx, measurement);
Eigen::Matrix2d information = Eigen::Matrix2d::Identity();
double size = 1.0;
multiFramePtr->getKeypointSize(camIdx, keypointIdx, size);
information *= 64.0 / (size * size);
// create error term
std::shared_ptr < ceres::ReprojectionError
< GEOMETRY_TYPE
>> reprojectionError(
new ceres::ReprojectionError<GEOMETRY_TYPE>(
multiFramePtr->template geometryAs<GEOMETRY_TYPE>(camIdx),
camIdx, measurement, information));
::ceres::ResidualBlockId retVal = mapPtr_->addResidualBlock(
reprojectionError,
cauchyLossFunctionPtr_ ? cauchyLossFunctionPtr_.get() : NULL,
mapPtr_->parameterBlockPtr(poseId),
mapPtr_->parameterBlockPtr(landmarkId),
mapPtr_->parameterBlockPtr(
statesMap_.at(poseId).sensors.at(SensorStates::Camera).at(camIdx).at(
CameraSensorStates::T_SCi).id));
// remember
landmarksMap_.at(landmarkId).observations.insert(
std::pair<okvis::KeypointIdentifier, uint64_t>(
kid, reinterpret_cast<uint64_t>(retVal)));
return retVal;
}