//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
* c1Rc2.matrix();
EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
// Check some projections
EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
// Check homogeneous version
EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
}
math::Vector<2> CDispRigidBody::bCoef(real_t e, const NewtonCollisionBase & bData) const
{
const real_t & dt = bData.dData.dt;
math::Vector<2> r(bData.points.eff - rotationPoint());
rotateToGlobal(r);
real_t ax = 1.0 / m() + (r(1) * r(1) / Idisp());
real_t ay = 1.0 / m() + (r(0) * r(0) / Idisp());
//compensate permanent forces
//reaction to permanent force
real_t bx = resultantForce(0) * dt * ax;
real_t by = resultantForce(1) * dt * ay;
//reaction to permanent moment
real_t resultantMomentx_add = 0.0;
real_t resultantMomenty_add = 0.0;
if (r(1) != 0.0)
resultantMomentx_add = resultantMoment() * dt * ax / r(1);
if (r(0) != 0.0)
resultantMomenty_add = resultantMoment() * dt * ay / r(0);
bx += resultantMomentx_add;
by -= resultantMomenty_add;
//momentum addition
bx += e * pA(0) / m();
by += e * pA(1) / m();
//angular momentum addition
bx += e * LA() * -r(1) / Idisp();
by += e * LA() * r(0) / Idisp();
//L translates into force proportional to (rx, ry)
real_t Lx_coef = std::abs(r(1)) / (std::abs(r(1)) + std::abs(r(0)));
real_t Ly_coef = std::abs(r(0)) / (std::abs(r(1)) + std::abs(r(0)));
real_t Lx_add = std::abs(LA() * r(1) / Idisp() + resultantMomentx_add);
real_t Ly_add = std::abs(LA() * r(0) / Idisp() - resultantMomenty_add);
real_t px_add = std::abs(pA(0) / m() + resultantForce(0) * dt * ax);
real_t py_add = std::abs(pA(1) / m() + resultantForce(1) * dt * ay);
//multiply bx by bx_coef
if ((Lx_add + px_add) != 0.0) //do not divide by 0.0 (note: px_add and Lx_add are absolute values)
bx *= (Lx_coef * Lx_add + px_add) / (Lx_add + px_add);
//multiply by by by_coef
if ((Ly_add + py_add) != 0.0) //do not divide by 0.0 (note: py_add and Ly_add are absolute values)
by *= (Ly_coef * Ly_add + py_add) / (Ly_add + py_add);
return math::Vector<2>(bx, by);
}
//*************************************************************************
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, extraMinimization) {
// Additional test with camera moving in positive X direction
// We start with a factor graph and add constraints to it
// Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < data.number_tracks(); i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K));
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
TEST (EssentialMatrixFactor3, minimization) {
// As before, we start with a factor graph and add constraints to it
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
// but now we specify the rotation bRc
graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2));
// Check error at ground truth
Values truth;
truth.insert(100, bodyE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
TEST (EssentialMatrixFactor2, minimization) {
// Here we want to optimize for E and inverse depths at the same time
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
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));
}
}
int tc_libcxx_utilities_util_smartptr_shared_const_shared_ptr_pointer(void)
{
B::count = 0;
A::count = 0;
{
std::shared_ptr<A> pA(new A);
TC_ASSERT_EXPR(pA.use_count() == 1);
{
B b;
std::shared_ptr<B> pB(pA, &b);
TC_ASSERT_EXPR(A::count == 1);
TC_ASSERT_EXPR(B::count == 2);
TC_ASSERT_EXPR(pA.use_count() == 2);
TC_ASSERT_EXPR(pB.use_count() == 2);
TC_ASSERT_EXPR(pB.get() == &b);
}
TC_ASSERT_EXPR(pA.use_count() == 1);
TC_ASSERT_EXPR(A::count == 1);
TC_ASSERT_EXPR(B::count == 1);
}
TC_ASSERT_EXPR(A::count == 0);
TC_ASSERT_EXPR(B::count == 0);
TC_SUCCESS_RESULT();
return 0;
}
int main()
{
{
std::shared_ptr<A> pA(new A);
assert(pA.use_count() == 1);
assert(A::count == 1);
{
std::shared_ptr<A> pA2(pA);
assert(A::count == 1);
assert(pA.use_count() == 2);
assert(pA2.use_count() == 2);
assert(pA2.get() == pA.get());
}
assert(pA.use_count() == 1);
assert(A::count == 1);
}
assert(A::count == 0);
{
std::shared_ptr<A> pA;
assert(pA.use_count() == 0);
assert(A::count == 0);
{
std::shared_ptr<A> pA2(pA);
assert(A::count == 0);
assert(pA.use_count() == 0);
assert(pA2.use_count() == 0);
assert(pA2.get() == pA.get());
}
assert(pA.use_count() == 0);
assert(A::count == 0);
}
assert(A::count == 0);
}
int Dense3d_MPI::setup()
{
//begin
pA(_srcPos!=NULL && _srcNor!=NULL && _trgPos!=NULL);
//--------------------------------------------------------------------------
/* Create a Scatter context - copies all source position values to each processor
* See http://www-unix.mcs.anl.gov/petsc/petsc-as/snapshots/petsc-current/docs/manualpages/Vec/VecScatterCreateToAll.html */
{
VecScatter ctx;
pC( VecScatterCreateToAll(_srcPos, &ctx, &_srcAllPos) );
// VecScatterBegin(VecScatter inctx,Vec x,Vec y,InsertMode addv,ScatterMode mode)
pC( VecScatterBegin(ctx, _srcPos, _srcAllPos, INSERT_VALUES, SCATTER_FORWARD) );
pC( VecScatterEnd(ctx, _srcPos, _srcAllPos, INSERT_VALUES, SCATTER_FORWARD ) );
pC( VecScatterDestroy(ctx) );
}
/* Create a Scatter context - copies all source normal values to each processor */
{
VecScatter ctx;
pC( VecScatterCreateToAll(_srcNor, &ctx, &_srcAllNor) );
pC( VecScatterBegin(ctx, _srcNor, _srcAllNor, INSERT_VALUES, SCATTER_FORWARD) );
pC( VecScatterEnd(ctx, _srcNor, _srcAllNor, INSERT_VALUES, SCATTER_FORWARD) );
pC( VecScatterDestroy(ctx) );
}
return(0);
}
bool EXPORT_API ProjectPointTriangleConstraintsJB(btVector3* p, btVector3* p0, btVector3* p1, btVector3* p2, float radius, float K1, float K2, btVector3* c, btVector3* c0, btVector3* c1, btVector3* c2, btVector3 *debugOut)
{
//btScalar massInv = 1.0f / mass;
Eigen::Vector3f pp(p->x(), p->y(), p->z());
Eigen::Vector3f pA(p0->x(), p0->y(), p0->z());
Eigen::Vector3f pB(p1->x(), p1->y(), p1->z());
Eigen::Vector3f pC(p2->x(), p2->y(), p2->z());
Eigen::Vector3f corrA;
Eigen::Vector3f corrB;
Eigen::Vector3f corrC;
Eigen::Vector3f corr;
Eigen::Vector3f debug;
const bool res = PBD::PositionBasedDynamics::solveTrianglePointDistConstraint(pp, 1, pA, 1, pB, 1, pC, 1, radius, K1, K2, corr, corrA, corrB, corrC);
if (res)
{
c->setValue(corr.x(), corr.y(), corr.z());
c0->setValue(corrA.x(), corrA.y(), corrA.z());
c1->setValue(corrB.x(), corrB.y(), corrB.z());
c2->setValue(corrC.x(), corrC.y(), corrC.z());
debugOut->setValue(debug.x(), debug.y(), debug.z());
}
return res;
}
void EXPORT_API ComputeInternalConstraintsJB(btVector3* predictedPositions, float* invMasses, bool* posLocks, Link* links, int linksCount, float Ks_prime, float mass)
{
for (int i = 0; i < linksCount; i++)
{
Link link = links[i];
if ((link.type == -1) || (posLocks[link.idA] && posLocks[link.idB]))
continue;
btScalar wA = posLocks[link.idA] ? 0.0f : invMasses[link.idA];
btScalar wB = posLocks[link.idB] ? 0.0f : invMasses[link.idB];
btVector3 predPosA = predictedPositions[link.idA];
btVector3 predPosB = predictedPositions[link.idB];
Eigen::Vector3f pA(predPosA.x(), predPosA.y(), predPosA.z());
Eigen::Vector3f pB(predPosB.x(), predPosB.y(), predPosB.z());
Eigen::Vector3f corrA;
Eigen::Vector3f corrB;
const bool res = PBD::PositionBasedDynamics::solveDistanceConstraint(pA, wA, pB, wB, link.restLength, Ks_prime, Ks_prime, corrA, corrB);
if (res)
{
if (wA != 0.0f)
predictedPositions[link.idA] += btVector3(corrA.x(), corrA.y(), corrA.z());
if (wB != 0.0f)
predictedPositions[link.idB] += btVector3(corrB.x(), corrB.y(), corrB.z());
}
}
}
int main()
{
static_assert(( std::is_convertible<std::shared_ptr<A>, std::shared_ptr<B> >::value), "");
static_assert((!std::is_convertible<std::shared_ptr<B>, std::shared_ptr<A> >::value), "");
static_assert((!std::is_convertible<std::shared_ptr<A>, std::shared_ptr<C> >::value), "");
{
std::shared_ptr<A> pA(new A);
assert(pA.use_count() == 1);
assert(B::count == 1);
assert(A::count == 1);
{
B* p = pA.get();
std::shared_ptr<B> pB(std::move(pA));
assert(B::count == 1);
assert(A::count == 1);
#ifndef _LIBCPP_HAS_NO_RVALUE_REFERENCES
assert(pB.use_count() == 1);
assert(pA.use_count() == 0);
#else // _LIBCPP_HAS_NO_RVALUE_REFERENCES
assert(pB.use_count() == 2);
assert(pA.use_count() == 2);
#endif // _LIBCPP_HAS_NO_RVALUE_REFERENCES
assert(p == pB.get());
}
#ifndef _LIBCPP_HAS_NO_RVALUE_REFERENCES
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
#else // _LIBCPP_HAS_NO_RVALUE_REFERENCES
assert(pA.use_count() == 1);
assert(B::count == 1);
assert(A::count == 1);
#endif // _LIBCPP_HAS_NO_RVALUE_REFERENCES
}
assert(B::count == 0);
assert(A::count == 0);
{
std::shared_ptr<A> pA;
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
{
std::shared_ptr<B> pB(pA);
assert(B::count == 0);
assert(A::count == 0);
assert(pB.use_count() == 0);
assert(pA.use_count() == 0);
assert(pA.get() == pB.get());
}
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
}
/* 任意のレジスターの値をスタックにプッシュする。
* 事前に stack_socket に値をセットせずに、ダイレクトで指定できるので、ソースが小さくなる
*/
void push_stack(const char* regname_data)
{
write_mem(regname_data, "stack_head");
pA("stack_head++;");
#ifdef DEBUG_STACK
pA_mes("push_stack(): ");
pA_reg("stack_head");
pA_mes("\\n");
#endif /* DEBUG_STACK */
}
/* スタックから任意のレジスターへ値をポップする。
* 事前に stack_socket に値をセットせずに、ダイレクトで指定できるので、ソースが小さくなる
*/
void pop_stack(const char* regname_data)
{
pA("stack_head--;");
read_mem(regname_data, "stack_head");
#ifdef DEBUG_STACK
pA_mes("pop_stack(): ");
pA_reg("stack_head");
pA_mes("\\n");
#endif /* DEBUG_STACK */
}
void pA()
{
switch (ch) {
case 'a':
pa(); pA();
break;
case 'b':
pb(); pB();
break;
default:
error();
}
}
int main()
{
boost::shared_ptr<A> pA(new A);
std::cout << pA.get() << std::endl;
boost::shared_ptr<A> pB(pA);
std::cout << pA.get() << std::endl;
std::cout << pB.get() << std::endl;
return 0;
}
/* 任意のレジスターの値へコールスタックからポップする
*/
void pop_callstack(const char* register_name)
{
pA("callstack_head--;");
read_mem(register_name, "callstack_head");
#ifdef DEBUG_CALLSTACK
pA_mes("pop_callstack(): ");
pA_reg(register_name);
pA_mes(", ");
pA_reg("callstack_head");
pA_mes("\\n");
#endif /* DEBUG_CALLSTACK */
}
/* 任意のレジスターの値をコールスタックへプッシュする
*/
void push_callstack(const char* register_name)
{
write_mem(register_name, "callstack_head");
pA("callstack_head++;");
#ifdef DEBUG_CALLSTACK
pA_mes("push_callstack(): ");
pA_reg(register_name);
pA_mes(", ");
pA_reg("callstack_head");
pA_mes("\\n");
#endif /* DEBUG_CALLSTACK */
}
Eigen::Matrix<T, Eigen::Dynamic, 1> VCLtoVecSEXP(SEXP A)
{
Rcpp::XPtr<viennacl::vector<T> > pA(A);
int M = pA->size();
std::cout << M << std::endl;
Eigen::Matrix<T, Eigen::Dynamic, 1> Am(M);
std::cout << Am << std::endl;
viennacl::copy(*pA, Am);
return Am;
}
DocumentSource::GetDepsReturn DocumentSourceGroup::getDependencies(set<string>& deps) const {
// add the _id
pIdExpression->addDependencies(deps);
// add the rest
const size_t n = vFieldName.size();
for(size_t i = 0; i < n; ++i) {
intrusive_ptr<Accumulator> pA((*vpAccumulatorFactory[i])(pExpCtx));
pA->addOperand(vpExpression[i]);
pA->addDependencies(deps);
}
return EXHAUSTIVE;
}
void DocumentSourceGroup::sourceToBson(BSONObjBuilder *pBuilder) const {
BSONObjBuilder insides;
/* add the _id */
pIdExpression->addToBsonObj(&insides, Document::idName.c_str(), 0);
/* add the remaining fields */
const size_t n = vFieldName.size();
for(size_t i = 0; i < n; ++i) {
intrusive_ptr<Accumulator> pA((*vpAccumulatorFactory[i])(pCtx));
pA->addOperand(vpExpression[i]);
pA->addToBsonObj(&insides, vFieldName[i], 0);
}
pBuilder->append(groupName, insides.done());
}
int tc_libcxx_utilities_util_smartptr_shared_const_shared_ptr_rv(void)
{
A::count = 0;
{
std::shared_ptr<A> pA(new A);
TC_ASSERT_EXPR(pA.use_count() == 1);
TC_ASSERT_EXPR(A::count == 1);
{
A* p = pA.get();
std::shared_ptr<A> pA2(std::move(pA));
TC_ASSERT_EXPR(A::count == 1);
#if TEST_STD_VER >= 11
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(pA2.use_count() == 1);
#else
TC_ASSERT_EXPR(pA.use_count() == 2);
TC_ASSERT_EXPR(pA2.use_count() == 2);
#endif
TC_ASSERT_EXPR(pA2.get() == p);
}
#if TEST_STD_VER >= 11
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(A::count == 0);
#else
TC_ASSERT_EXPR(pA.use_count() == 1);
TC_ASSERT_EXPR(A::count == 1);
#endif
}
TC_ASSERT_EXPR(A::count == 0);
{
std::shared_ptr<A> pA;
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(A::count == 0);
{
std::shared_ptr<A> pA2(std::move(pA));
TC_ASSERT_EXPR(A::count == 0);
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(pA2.use_count() == 0);
TC_ASSERT_EXPR(pA2.get() == pA.get());
}
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(A::count == 0);
}
TC_ASSERT_EXPR(A::count == 0);
TC_SUCCESS_RESULT();
return 0;
}
//*************************************************************************
TEST (EssentialMatrixFactor, minimization) {
// Here we want to optimize directly on essential matrix constraints
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
// but GTSAM does the equivalent anyway, provided we give the right
// factors. In this case, the factors are the constraints.
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));
// Check error at ground truth
Values truth;
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual, 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
}
/** @brief Constructor
* @param A matrix whose approximate inverse is calculated. Must be quadratic.
* @param tag SPAI configuration tag
*/
fspai_precond(const MatrixType & A,
const fspai_tag & tag) : tag_(tag), L(viennacl::traits::context(A)), L_trans(viennacl::traits::context(A)), temp_apply_vec_(A.size1(), viennacl::traits::context(A))
{
//UBLASSparseMatrixType ubls_A;
UBLASSparseMatrixType ublas_A(A.size1(), A.size2());
UBLASSparseMatrixType pA(A.size1(), A.size2());
UBLASSparseMatrixType ublas_L(A.size1(), A.size2());
UBLASSparseMatrixType ublas_L_trans(A.size1(), A.size2());
viennacl::copy(A, ublas_A);
//viennacl::copy(ubls_A, vcl_A);
//vcl_At = viennacl::linalg::prod(vcl_A, vcl_A);
//vcl_pA = viennacl::linalg::prod(vcl_A, vcl_At);
//viennacl::copy(vcl_pA, pA);
pA = ublas_A;
//execute SPAI with ublas matrix types
viennacl::linalg::detail::spai::computeFSPAI(ublas_A, pA, ublas_L, ublas_L_trans, tag_);
//copy back to GPU
viennacl::copy(ublas_L, L);
viennacl::copy(ublas_L_trans, L_trans);
}
//*************************************************************************************
void CConsoleAlarmAlertSession::SetAlarmL(const RMessage2& aMessage)
{
TPckg<TASShdAlarm> pA(iAlarm);
aMessage.ReadL(KSlot0, pA);
if (iAlarm.HasAssociatedData())
{
iAlarmAssociatedDataSize = aMessage.GetDesLength(2);
TPckg<TAgnAlarmInfo> pB(iAlarmData);
aMessage.ReadL(KSlot2, pB);
//Storing the data in the server for the test session to read..
HBufC8* data = HBufC8::NewLC(iAlarmAssociatedDataSize);
TPtr8 pData(data->Des());
aMessage.ReadL(KSlot2, pData);
iServer->SetAttachment(data); //Server takes an ownership
CleanupStack::Pop(data);
}
else
iAlarmAssociatedDataSize = 0;
}
//*************************************************************************
TEST (EssentialMatrixFactor, extraMinimization) {
// Additional test with camera moving in positive X direction
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K));
// Check error at ground truth
Values truth;
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual, 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
}
/** @brief Constructor
* @param A matrix whose approximate inverse is calculated. Must be quadratic.
* @param tag spai tag
*/
spai_precond(const MatrixType& A,
const spai_tag& tag): tag_(tag){
//VCLMatrixType vcl_Ap((unsigned int)A.size2(), (unsigned int)A.size1()), vcl_A((unsigned int)A.size1(), (unsigned int)A.size2()),
//vcl_At((unsigned int)A.size1(), (unsigned int)A.size2());
//UBLASDenseMatrixType dA = A;
MatrixType pA(A.size1(), A.size2());
MatrixType At;
//std::cout<<A<<std::endl;
if(!tag_.getIsRight()){
viennacl::linalg::detail::spai::sparse_transpose(A, At);
}else{
At = A;
}
pA = At;
viennacl::linalg::detail::spai::initPreconditioner(pA, spai_m_);
viennacl::linalg::detail::spai::computeSPAI(At, spai_m_, tag_);
//(At, pA, tag_.getIsRight(), tag_.getIsStatic(), (ScalarType)_tag.getResidualNormThreshold(), (unsigned int)_tag.getIterationLimit(),
//_spai_m);
}
//*************************************************************************
TEST (EssentialMatrixFactor, factor) {
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor factor(1, pA(i), pB(i), model1);
// Check evaluation
Vector expected(1);
expected << 0;
Matrix Hactual;
Vector actual = factor.evaluateError(trueE, Hactual);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected;
typedef Eigen::Matrix<double,1,1> Vector1;
Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
}
}
static SCE_PFX_FORCE_INLINE
bool pfxContactTriangleConvex(PfxContactCache &contacts,PfxUInt32 facetId,
const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
PfxUInt32 edgeChk,
const PfxConvexMesh &convex)
{
PfxVector3 facetPnts[6] = {
p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
};
PfxPoint3 pA(0.0f),pB(0.0f);
PfxVector3 nml(0.0f);
PfxGjkSolver gjk;
gjk.setup((void*)facetPnts,(void*)&convex,pfxGetSupportVertexTriangleWithThickness,pfxGetSupportVertexConvex);
PfxFloat d = gjk.collide(nml,pA,pB,PfxTransform3::identity(),PfxTransform3::identity(),SCE_PFX_FLT_MAX);
if(d >= 0.0f) return false;
PfxVector3 pointsOnTriangle = PfxVector3(pA);
PfxVector3 pointsOnConvex = PfxVector3(pB);
PfxVector3 axis = nml;
// 面上の最近接点が凸エッジ上でない場合は法線を変える
if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
axis=-normal;
}
PfxSubData subData;
subData.setFacetId(facetId);
contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);
return true;
}
