//*************************************************************************
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);
}
//*************************************************************************
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));
}
}
//*************************************************************************
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));
}
}
/* スタックの初期化
*/
void init_stack(void)
{
pB("SInt32 stack_head:R01;");
pB("SInt32 stack_socket:R03;");
pB("SInt32 stack_tmp:R21;");
pB("stack_head = %d;", STACK_BEGIN_ADDRESS);
}
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);
}
int tc_libcxx_utilities_util_smartptr_shared_const_shared_ptr_Y(void)
{
B::count = 0;
A::count = 0;
C::count = 0;
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), "");
{
const std::shared_ptr<A> pA(new A);
TC_ASSERT_EXPR(pA.use_count() == 1);
TC_ASSERT_EXPR(B::count == 1);
TC_ASSERT_EXPR(A::count == 1);
{
std::shared_ptr<B> pB(pA);
TC_ASSERT_EXPR(B::count == 1);
TC_ASSERT_EXPR(A::count == 1);
TC_ASSERT_EXPR(pB.use_count() == 2);
TC_ASSERT_EXPR(pA.use_count() == 2);
TC_ASSERT_EXPR(pA.get() == pB.get());
}
TC_ASSERT_EXPR(pA.use_count() == 1);
TC_ASSERT_EXPR(B::count == 1);
TC_ASSERT_EXPR(A::count == 1);
}
TC_ASSERT_EXPR(B::count == 0);
TC_ASSERT_EXPR(A::count == 0);
{
std::shared_ptr<A> pA;
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(B::count == 0);
TC_ASSERT_EXPR(A::count == 0);
{
std::shared_ptr<B> pB(pA);
TC_ASSERT_EXPR(B::count == 0);
TC_ASSERT_EXPR(A::count == 0);
TC_ASSERT_EXPR(pB.use_count() == 0);
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(pA.get() == pB.get());
}
TC_ASSERT_EXPR(pA.use_count() == 0);
TC_ASSERT_EXPR(B::count == 0);
TC_ASSERT_EXPR(A::count == 0);
}
TC_ASSERT_EXPR(B::count == 0);
TC_ASSERT_EXPR(A::count == 0);
TC_SUCCESS_RESULT();
return 0;
}
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;
}
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());
}
}
}
//*************************************************************************
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);
}
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;
}
/*!
\ingroup group_imgproc_histogram
Adjust the contrast of a color image by performing an histogram equalization.
The intensity distribution is redistributed over the full [0 - 255] range such as the cumulative histogram
distribution becomes linear. The alpha channel is ignored / copied from the source alpha channel.
\param I : The color image to apply histogram equalization.
\param useHSV : If true, the histogram equalization is performed on the value channel (in HSV space), otherwise
the histogram equalization is performed independently on the RGB channels.
*/
void vp::equalizeHistogram(vpImage<vpRGBa> &I, const bool useHSV) {
if(I.getWidth()*I.getHeight() == 0) {
return;
}
if(!useHSV) {
//Split the RGBa image into 4 images
vpImage<unsigned char> pR(I.getHeight(), I.getWidth());
vpImage<unsigned char> pG(I.getHeight(), I.getWidth());
vpImage<unsigned char> pB(I.getHeight(), I.getWidth());
vpImage<unsigned char> pa(I.getHeight(), I.getWidth());
vpImageConvert::split(I, &pR, &pG, &pB, &pa);
//Apply histogram equalization for each channel
vp::equalizeHistogram(pR);
vp::equalizeHistogram(pG);
vp::equalizeHistogram(pB);
//Merge the result in I
unsigned int size = I.getWidth()*I.getHeight();
unsigned char *ptrStart = (unsigned char*) I.bitmap;
unsigned char *ptrEnd = ptrStart + size*4;
unsigned char *ptrCurrent = ptrStart;
unsigned int cpt = 0;
while(ptrCurrent != ptrEnd) {
*ptrCurrent = pR.bitmap[cpt];
++ptrCurrent;
*ptrCurrent = pG.bitmap[cpt];
++ptrCurrent;
*ptrCurrent = pB.bitmap[cpt];
++ptrCurrent;
*ptrCurrent = pa.bitmap[cpt];
++ptrCurrent;
cpt++;
}
} else {
vpImage<unsigned char> hue(I.getHeight(), I.getWidth());
vpImage<unsigned char> saturation(I.getHeight(), I.getWidth());
vpImage<unsigned char> value(I.getHeight(), I.getWidth());
unsigned int size = I.getWidth()*I.getHeight();
//Convert from RGBa to HSV
vpImageConvert::RGBaToHSV((unsigned char *) I.bitmap, (unsigned char *) hue.bitmap,
(unsigned char *) saturation.bitmap, (unsigned char *) value.bitmap, size);
//Histogram equalization on the value plane
vp::equalizeHistogram(value);
//Convert from HSV to RGBa
vpImageConvert::HSVToRGBa((unsigned char*) hue.bitmap, (unsigned char*) saturation.bitmap,
(unsigned char*) value.bitmap, (unsigned char*) I.bitmap, size);
}
}
int main(int, char**)
{
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), "");
{
const std::shared_ptr<A> pA(new A);
assert(pA.use_count() == 1);
assert(B::count == 1);
assert(A::count == 1);
{
std::shared_ptr<B> pB(pA);
assert(B::count == 1);
assert(A::count == 1);
assert(pB.use_count() == 2);
assert(pA.use_count() == 2);
assert(pA.get() == pB.get());
}
assert(pA.use_count() == 1);
assert(B::count == 1);
assert(A::count == 1);
}
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);
return 0;
}
int main()
{
static_assert(( std::is_convertible<std::weak_ptr<A>, std::weak_ptr<B> >::value), "");
static_assert((!std::is_convertible<std::weak_ptr<B>, std::weak_ptr<A> >::value), "");
static_assert((!std::is_convertible<std::weak_ptr<A>, std::weak_ptr<C> >::value), "");
{
const std::weak_ptr<A> pA(std::shared_ptr<A>(new A));
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
{
std::weak_ptr<B> pB(pA);
assert(B::count == 0);
assert(A::count == 0);
assert(pB.use_count() == 0);
assert(pA.use_count() == 0);
}
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
{
std::weak_ptr<A> pA;
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
{
std::weak_ptr<B> pB(pA);
assert(B::count == 0);
assert(A::count == 0);
assert(pB.use_count() == 0);
assert(pA.use_count() == 0);
}
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
}
int main()
{
{
const std::shared_ptr<A> ps(new A);
const std::weak_ptr<A> pA(ps);
assert(pA.use_count() == 1);
assert(B::count == 1);
assert(A::count == 1);
{
std::weak_ptr<A> pB(pA);
assert(B::count == 1);
assert(A::count == 1);
assert(pB.use_count() == 1);
assert(pA.use_count() == 1);
}
assert(pA.use_count() == 1);
assert(B::count == 1);
assert(A::count == 1);
}
assert(B::count == 0);
assert(A::count == 0);
{
std::weak_ptr<A> pA;
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
{
std::weak_ptr<A> pB(pA);
assert(B::count == 0);
assert(A::count == 0);
assert(pB.use_count() == 0);
assert(pA.use_count() == 0);
}
assert(pA.use_count() == 0);
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
}
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 pB()
{
switch (ch) {
case 'b':
pb(); pB();
break;
case 'c':
pc(); pC();
break;
default :
error();
}
}
CXMLmessageBook *COARfile::GetMessageBook()
{
if(!m_bCheckedMsgBook)
{
m_bCheckedMsgBook = true;
wxString sFileName = FindMessageBookFile();
if(!sFileName.IsEmpty())
{
auto_ptr<CXMLmessageBook> pB(new CXMLmessageBook());
if(pB->LoadFile(sFileName) && pB->IsOK())
{
m_pMsgBook = pB.release();
}
}
}
return m_pMsgBook;
}
//*************************************************************************
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);
}
//*************************************************************************************
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);
}
//*************************************************************************
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;
}
int main(int, char**)
{
{
std::unique_ptr<A> pA(new A);
A* ptrA = pA.get();
{
std::shared_ptr<B> pB(new B);
pB = std::move(pA);
assert(B::count == 1);
assert(A::count == 1);
assert(pB.use_count() == 1);
assert(pA.get() == 0);
assert(pB.get() == ptrA);
}
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
{
std::unique_ptr<A> pA;
A* ptrA = pA.get();
{
std::shared_ptr<B> pB(new B);
pB = std::move(pA);
assert(B::count == 0);
assert(A::count == 0);
// assert(pB.use_count() == 1); // no longer true due to LWG 2415
assert(pA.get() == 0);
assert(pB.get() == ptrA);
}
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
{
std::unique_ptr<A> pA(new A);
A* ptrA = pA.get();
{
std::shared_ptr<B> pB;
pB = std::move(pA);
assert(B::count == 1);
assert(A::count == 1);
assert(pB.use_count() == 1);
assert(pA.get() == 0);
assert(pB.get() == ptrA);
}
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
{
std::unique_ptr<A> pA;
A* ptrA = pA.get();
{
std::shared_ptr<B> pB;
pB = std::move(pA);
assert(B::count == 0);
assert(A::count == 0);
// assert(pB.use_count() == 1); // no longer true due to LWG 2415
assert(pA.get() == 0);
assert(pB.get() == ptrA);
}
assert(B::count == 0);
assert(A::count == 0);
}
assert(B::count == 0);
assert(A::count == 0);
return 0;
}
void tst_QHashFunctions::qhash()
{
{
QBitArray a1;
QBitArray a2;
QVERIFY(qHash(a1) == 0);
a1.resize(1);
a1.setBit(0, true);
a2.resize(1);
a2.setBit(0, false);
uint h1 = qHash(a1);
uint h2 = qHash(a2);
QVERIFY(h1 != h2);
a2.setBit(0, true);
QVERIFY(h1 == qHash(a2));
a1.fill(true, 8);
a1.resize(7);
h1 = qHash(a1);
a2.fill(true, 7);
h2 = qHash(a2);
QVERIFY(h1 == h2);
a2.setBit(0, false);
uint h3 = qHash(a2);
QVERIFY(h2 != h3);
a2.setBit(0, true);
QVERIFY(h2 == qHash(a2));
a2.setBit(6, false);
uint h4 = qHash(a2);
QVERIFY(h2 != h4);
a2.setBit(6, true);
QVERIFY(h2 == qHash(a2));
QVERIFY(h3 != h4);
}
{
QPair<int, int> p12(1, 2);
QPair<int, int> p21(2, 1);
QVERIFY(qHash(p12) == qHash(p12));
QVERIFY(qHash(p21) == qHash(p21));
QVERIFY(qHash(p12) != qHash(p21));
QPair<int, int> pA(0x12345678, 0x12345678);
QPair<int, int> pB(0x12345675, 0x12345675);
QVERIFY(qHash(pA) != qHash(pB));
}
}
/*!
\ingroup group_imgproc_contrast
Stretch the contrast of a color image.
\param I : The color image to stretch the contrast.
*/
void vp::stretchContrast(vpImage<vpRGBa> &I) {
//Find min and max intensity values
vpRGBa min = 255, max = 0;
//Split the RGBa image into 4 images
vpImage<unsigned char> pR(I.getHeight(), I.getWidth());
vpImage<unsigned char> pG(I.getHeight(), I.getWidth());
vpImage<unsigned char> pB(I.getHeight(), I.getWidth());
vpImage<unsigned char> pa(I.getHeight(), I.getWidth());
vpImageConvert::split(I, &pR, &pG, &pB, &pa);
//Min max values calculated for each channel
unsigned char minChannel, maxChannel;
pR.getMinMaxValue(minChannel, maxChannel);
min.R = minChannel;
max.R = maxChannel;
pG.getMinMaxValue(minChannel, maxChannel);
min.G = minChannel;
max.G = maxChannel;
pB.getMinMaxValue(minChannel, maxChannel);
min.B = minChannel;
max.B = maxChannel;
pa.getMinMaxValue(minChannel, maxChannel);
min.A = minChannel;
max.A = maxChannel;
//Construct the look-up table
vpRGBa lut[256];
unsigned char rangeR = max.R - min.R;
if(rangeR > 0) {
for(unsigned int x = min.R; x <= max.R; x++) {
lut[x].R = 255 * (x - min.R) / rangeR;
}
} else {
lut[min.R].R = min.R;
}
unsigned char rangeG = max.G - min.G;
if(rangeG > 0) {
for(unsigned int x = min.G; x <= max.G; x++) {
lut[x].G = 255 * (x - min.G) / rangeG;
}
} else {
lut[min.G].G = min.G;
}
unsigned char rangeB = max.B - min.B;
if(rangeB > 0) {
for(unsigned int x = min.B; x <= max.B; x++) {
lut[x].B = 255 * (x - min.B) / rangeB;
}
} else {
lut[min.B].B = min.B;
}
unsigned char rangeA = max.A - min.A;
if(rangeA > 0) {
for(unsigned int x = min.A; x <= max.A; x++) {
lut[x].A = 255 * (x - min.A) / rangeA;
}
} else {
lut[min.A].A = min.A;
}
I.performLut(lut);
}
bool test(bool is_kernel_exact = true)
{
// types
typedef typename K::FT FT;
typedef typename K::Line_3 Line;
typedef typename K::Point_3 Point;
typedef typename K::Segment_3 Segment;
typedef typename K::Ray_3 Ray;
typedef typename K::Line_3 Line;
typedef typename K::Triangle_3 Triangle;
/* -------------------------------------
// Test data is something like that (in t supporting plane)
// Triangle is (p1,p2,p3)
//
// +E +1
// / \
// +C 6+ +8 +4 +B
// / 9++7 \
// 3+-------+5--+2
//
// +F +A
------------------------------------- */
Point p1(FT(1.), FT(0.), FT(0.));
Point p2(FT(0.), FT(1.), FT(0.));
Point p3(FT(0.), FT(0.), FT(1.));
Triangle t(p1,p2,p3);
// Edges of t
Segment s12(p1,p2);
Segment s21(p2,p1);
Segment s13(p1,p3);
Segment s23(p2,p3);
Segment s32(p3,p2);
Segment s31(p3,p1);
bool b = test_aux(is_kernel_exact,t,s12,"t-s12",s12);
b &= test_aux(is_kernel_exact,t,s21,"t-s21",s21);
b &= test_aux(is_kernel_exact,t,s13,"t-s13",s13);
b &= test_aux(is_kernel_exact,t,s23,"t-s23",s23);
// Inside points
Point p4(FT(0.5), FT(0.5), FT(0.));
Point p5(FT(0.), FT(0.75), FT(0.25));
Point p6(FT(0.5), FT(0.), FT(0.5));
Point p7(FT(0.25), FT(0.625), FT(0.125));
Point p8(FT(0.5), FT(0.25), FT(0.25));
Segment s14(p1,p4);
Segment s41(p4,p1);
Segment s24(p2,p4);
Segment s42(p4,p2);
Segment s15(p1,p5);
Segment s25(p2,p5);
Segment s34(p3,p4);
Segment s35(p3,p5);
Segment s36(p3,p6);
Segment s45(p4,p5);
Segment s16(p1,p6);
Segment s26(p2,p6);
Segment s62(p6,p2);
Segment s46(p4,p6);
Segment s48(p4,p8);
Segment s56(p5,p6);
Segment s65(p6,p5);
Segment s64(p6,p4);
Segment s17(p1,p7);
Segment s67(p6,p7);
Segment s68(p6,p8);
Segment s86(p8,p6);
Segment s78(p7,p8);
Segment s87(p8,p7);
b &= test_aux(is_kernel_exact,t,s14,"t-s14",s14);
b &= test_aux(is_kernel_exact,t,s41,"t-s41",s41);
b &= test_aux(is_kernel_exact,t,s24,"t-s24",s24);
b &= test_aux(is_kernel_exact,t,s42,"t-s42",s42);
b &= test_aux(is_kernel_exact,t,s15,"t-s15",s15);
b &= test_aux(is_kernel_exact,t,s25,"t-s25",s25);
b &= test_aux(is_kernel_exact,t,s34,"t-s34",s34);
b &= test_aux(is_kernel_exact,t,s35,"t-s35",s35);
b &= test_aux(is_kernel_exact,t,s36,"t-s36",s36);
b &= test_aux(is_kernel_exact,t,s45,"t-s45",s45);
b &= test_aux(is_kernel_exact,t,s16,"t-s16",s16);
b &= test_aux(is_kernel_exact,t,s26,"t-s26",s26);
b &= test_aux(is_kernel_exact,t,s62,"t-s62",s62);
b &= test_aux(is_kernel_exact,t,s46,"t-s46",s46);
b &= test_aux(is_kernel_exact,t,s65,"t-s65",s65);
b &= test_aux(is_kernel_exact,t,s64,"t-s64",s64);
b &= test_aux(is_kernel_exact,t,s48,"t-s48",s48);
b &= test_aux(is_kernel_exact,t,s56,"t-s56",s56);
b &= test_aux(is_kernel_exact,t,s17,"t-t17",s17);
b &= test_aux(is_kernel_exact,t,s67,"t-t67",s67);
b &= test_aux(is_kernel_exact,t,s68,"t-s68",s68);
b &= test_aux(is_kernel_exact,t,s86,"t-s86",s86);
b &= test_aux(is_kernel_exact,t,s78,"t-t78",s78);
b &= test_aux(is_kernel_exact,t,s87,"t-t87",s87);
// Outside points (in triangle plane)
Point pA(FT(-0.5), FT(1.), FT(0.5));
Point pB(FT(0.5), FT(1.), FT(-0.5));
Point pC(FT(0.5), FT(-0.5), FT(1.));
Point pE(FT(1.), FT(-1.), FT(1.));
Point pF(FT(-1.), FT(0.), FT(2.));
Segment sAB(pA,pB);
Segment sBC(pB,pC);
Segment s2E(p2,pE);
Segment sE2(pE,p2);
Segment s2A(p2,pA);
Segment s6E(p6,pE);
Segment sB8(pB,p8);
Segment sC8(pC,p8);
Segment s8C(p8,pC);
Segment s1F(p1,pF);
Segment sF6(pF,p6);
b &= test_aux(is_kernel_exact,t,sAB,"t-sAB",p2);
b &= test_aux(is_kernel_exact,t,sBC,"t-sBC",s46);
b &= test_aux(is_kernel_exact,t,s2E,"t-s2E",s26);
b &= test_aux(is_kernel_exact,t,sE2,"t-sE2",s62);
b &= test_aux(is_kernel_exact,t,s2A,"t-s2A",p2);
b &= test_aux(is_kernel_exact,t,s6E,"t-s6E",p6);
b &= test_aux(is_kernel_exact,t,sB8,"t-sB8",s48);
b &= test_aux(is_kernel_exact,t,sC8,"t-sC8",s68);
b &= test_aux(is_kernel_exact,t,s8C,"t-s8C",s86);
b &= test_aux(is_kernel_exact,t,s1F,"t-s1F",s13);
b &= test_aux(is_kernel_exact,t,sF6,"t-sF6",s36);
// Outside triangle plane
Point pa(FT(0.), FT(0.), FT(0.));
Point pb(FT(2.), FT(0.), FT(0.));
Point pc(FT(1.), FT(0.), FT(1.));
Point pe(FT(1.), FT(0.5), FT(0.5));
Segment sab(pa,pb);
Segment sac(pa,pc);
Segment sae(pa,pe);
Segment sa8(pa,p8);
Segment sb2(pb,p2);
b &= test_aux(is_kernel_exact,t,sab,"t-sab",p1);
b &= test_aux(is_kernel_exact,t,sac,"t-sac",p6);
b &= test_aux(is_kernel_exact,t,sae,"t-sae",p8);
b &= test_aux(is_kernel_exact,t,sa8,"t-sa8",p8);
b &= test_aux(is_kernel_exact,t,sb2,"t-sb2",p2);
// -----------------------------------
// ray queries
// -----------------------------------
// Edges of t
Ray r12(p1,p2);
Ray r21(p2,p1);
Ray r13(p1,p3);
Ray r23(p2,p3);
b &= test_aux(is_kernel_exact,t,r12,"t-r12",s12);
b &= test_aux(is_kernel_exact,t,r21,"t-r21",s21);
b &= test_aux(is_kernel_exact,t,r13,"t-r13",s13);
b &= test_aux(is_kernel_exact,t,r23,"t-r23",s23);
// In triangle
Point p9_(FT(0.), FT(0.5), FT(0.5));
Point p9(FT(0.25), FT(0.375), FT(0.375));
Ray r14(p1,p4);
Ray r41(p4,p1);
Ray r24(p2,p4);
Ray r42(p4,p2);
Ray r15(p1,p5);
Ray r25(p2,p5);
Ray r34(p3,p4);
Ray r35(p3,p5);
Ray r36(p3,p6);
Ray r45(p4,p5);
Ray r16(p1,p6);
Ray r26(p2,p6);
Ray r62(p6,p2);
Ray r46(p4,p6);
Ray r48(p4,p8);
Ray r56(p5,p6);
Ray r47(p4,p7);
Ray r89(p8,p9);
Ray r86(p8,p6);
Ray r68(p6,p8);
Segment r89_res(p8,p9_);
b &= test_aux(is_kernel_exact,t,r14,"t-r14",s12);
b &= test_aux(is_kernel_exact,t,r41,"t-r41",s41);
b &= test_aux(is_kernel_exact,t,r24,"t-r24",s21);
b &= test_aux(is_kernel_exact,t,r42,"t-r42",s42);
b &= test_aux(is_kernel_exact,t,r15,"t-r15",s15);
b &= test_aux(is_kernel_exact,t,r25,"t-r25",s23);
b &= test_aux(is_kernel_exact,t,r34,"t-r34",s34);
b &= test_aux(is_kernel_exact,t,r35,"t-r35",s32);
b &= test_aux(is_kernel_exact,t,r36,"t-r36",s31);
b &= test_aux(is_kernel_exact,t,r45,"t-r45",s45);
b &= test_aux(is_kernel_exact,t,r16,"t-r16",s13);
b &= test_aux(is_kernel_exact,t,r26,"t-r26",s26);
b &= test_aux(is_kernel_exact,t,r62,"t-r62",s62);
b &= test_aux(is_kernel_exact,t,r46,"t-r46",s46);
b &= test_aux(is_kernel_exact,t,r48,"t-r48",s46);
b &= test_aux(is_kernel_exact,t,r56,"t-r56",s56);
b &= test_aux(is_kernel_exact,t,r47,"t-r47",s45);
b &= test_aux(is_kernel_exact,t,r89,"t-t89",r89_res);
b &= test_aux(is_kernel_exact,t,r68,"t-r68",s64);
b &= test_aux(is_kernel_exact,t,r86,"t-r86",s86);
// Outside points (in triangre prane)
Ray rAB(pA,pB);
Ray rBC(pB,pC);
Ray r2E(p2,pE);
Ray rE2(pE,p2);
Ray r2A(p2,pA);
Ray r6E(p6,pE);
Ray rB8(pB,p8);
Ray rC8(pC,p8);
Ray r8C(p8,pC);
Ray r1F(p1,pF);
Ray rF6(pF,p6);
b &= test_aux(is_kernel_exact,t,rAB,"t-rAB",p2);
b &= test_aux(is_kernel_exact,t,rBC,"t-rBC",s46);
b &= test_aux(is_kernel_exact,t,r2E,"t-r2E",s26);
b &= test_aux(is_kernel_exact,t,rE2,"t-rE2",s62);
b &= test_aux(is_kernel_exact,t,r2A,"t-r2A",p2);
b &= test_aux(is_kernel_exact,t,r6E,"t-r6E",p6);
b &= test_aux(is_kernel_exact,t,rB8,"t-rB8",s46);
b &= test_aux(is_kernel_exact,t,rC8,"t-rC8",s64);
b &= test_aux(is_kernel_exact,t,r8C,"t-r8C",s86);
b &= test_aux(is_kernel_exact,t,r1F,"t-r1F",s13);
b &= test_aux(is_kernel_exact,t,rF6,"t-rF6",s31);
// Outside triangle plane
Ray rab(pa,pb);
Ray rac(pa,pc);
Ray rae(pa,pe);
Ray ra8(pa,p8);
Ray rb2(pb,p2);
b &= test_aux(is_kernel_exact,t,rab,"t-rab",p1);
b &= test_aux(is_kernel_exact,t,rac,"t-rac",p6);
b &= test_aux(is_kernel_exact,t,rae,"t-rae",p8);
b &= test_aux(is_kernel_exact,t,ra8,"t-ra8",p8);
b &= test_aux(is_kernel_exact,t,rb2,"t-rb2",p2);
// -----------------------------------
// Line queries
// -----------------------------------
// Edges of t
Line l12(p1,p2);
Line l21(p2,p1);
Line l13(p1,p3);
Line l23(p2,p3);
b &= test_aux(is_kernel_exact,t,l12,"t-l12",s12);
b &= test_aux(is_kernel_exact,t,l21,"t-l21",s21);
b &= test_aux(is_kernel_exact,t,l13,"t-l13",s13);
b &= test_aux(is_kernel_exact,t,l23,"t-l23",s23);
// In triangle
Line l14(p1,p4);
Line l41(p4,p1);
Line l24(p2,p4);
Line l42(p4,p2);
Line l15(p1,p5);
Line l25(p2,p5);
Line l34(p3,p4);
Line l35(p3,p5);
Line l36(p3,p6);
Line l45(p4,p5);
Line l16(p1,p6);
Line l26(p2,p6);
Line l62(p6,p2);
Line l46(p4,p6);
Line l48(p4,p8);
Line l56(p5,p6);
Line l47(p4,p7);
Line l89(p8,p9);
Line l86(p8,p6);
Line l68(p6,p8);
Segment l89_res(p1,p9_);
b &= test_aux(is_kernel_exact,t,l14,"t-l14",s12);
b &= test_aux(is_kernel_exact,t,l41,"t-l41",s21);
b &= test_aux(is_kernel_exact,t,l24,"t-l24",s21);
b &= test_aux(is_kernel_exact,t,l42,"t-l42",s12);
b &= test_aux(is_kernel_exact,t,l15,"t-l15",s15);
b &= test_aux(is_kernel_exact,t,l25,"t-l25",s23);
b &= test_aux(is_kernel_exact,t,l34,"t-l34",s34);
b &= test_aux(is_kernel_exact,t,l35,"t-l35",s32);
b &= test_aux(is_kernel_exact,t,l36,"t-l36",s31);
b &= test_aux(is_kernel_exact,t,l45,"t-l45",s45);
b &= test_aux(is_kernel_exact,t,l16,"t-l16",s13);
b &= test_aux(is_kernel_exact,t,l26,"t-l26",s26);
b &= test_aux(is_kernel_exact,t,l62,"t-l62",s62);
b &= test_aux(is_kernel_exact,t,l46,"t-l46",s46);
b &= test_aux(is_kernel_exact,t,l48,"t-l48",s46);
b &= test_aux(is_kernel_exact,t,l56,"t-l56",s56);
b &= test_aux(is_kernel_exact,t,l47,"t-l47",s45);
b &= test_aux(is_kernel_exact,t,l89,"t-t89",l89_res);
b &= test_aux(is_kernel_exact,t,l68,"t-l68",s64);
b &= test_aux(is_kernel_exact,t,l86,"t-l86",s46);
// Outside points (in triangle plane)
Line lAB(pA,pB);
Line lBC(pB,pC);
Line l2E(p2,pE);
Line lE2(pE,p2);
Line l2A(p2,pA);
Line l6E(p6,pE);
Line lB8(pB,p8);
Line lC8(pC,p8);
Line l8C(p8,pC);
Line l1F(p1,pF);
Line lF6(pF,p6);
b &= test_aux(is_kernel_exact,t,lAB,"t-lAB",p2);
b &= test_aux(is_kernel_exact,t,lBC,"t-lBC",s46);
b &= test_aux(is_kernel_exact,t,l2E,"t-l2E",s26);
b &= test_aux(is_kernel_exact,t,lE2,"t-lE2",s62);
b &= test_aux(is_kernel_exact,t,l2A,"t-l2A",p2);
b &= test_aux(is_kernel_exact,t,l6E,"t-l6E",s26);
b &= test_aux(is_kernel_exact,t,lB8,"t-lB8",s46);
b &= test_aux(is_kernel_exact,t,lC8,"t-lC8",s64);
b &= test_aux(is_kernel_exact,t,l8C,"t-l8C",s46);
b &= test_aux(is_kernel_exact,t,l1F,"t-l1F",s13);
b &= test_aux(is_kernel_exact,t,lF6,"t-lF6",s31);
// Outside triangle plane
Line lab(pa,pb);
Line lac(pa,pc);
Line lae(pa,pe);
Line la8(pa,p8);
Line lb2(pb,p2);
b &= test_aux(is_kernel_exact,t,lab,"t-lab",p1);
b &= test_aux(is_kernel_exact,t,lac,"t-lac",p6);
b &= test_aux(is_kernel_exact,t,lae,"t-lae",p8);
b &= test_aux(is_kernel_exact,t,la8,"t-la8",p8);
b &= test_aux(is_kernel_exact,t,lb2,"t-lb2",p2);
return b;
}
inline void
LU( DistMatrix<F>& A, DistMatrix<int,VC,STAR>& p )
{
#ifndef RELEASE
CallStackEntry entry("LU");
if( A.Grid() != p.Grid() )
throw std::logic_error("{A,p} must be distributed over the same grid");
if( p.Viewing() &&
(std::min(A.Height(),A.Width()) != p.Height() || p.Width() != 1) )
throw std::logic_error
("p must be a vector of the same height as the min dimension of A.");
#endif
const Grid& g = A.Grid();
if( !p.Viewing() )
p.ResizeTo( std::min(A.Height(),A.Width()), 1 );
// Matrix views
DistMatrix<F>
ATL(g), ATR(g), A00(g), A01(g), A02(g), AB(g),
ABL(g), ABR(g), A10(g), A11(g), A12(g),
A20(g), A21(g), A22(g);
DistMatrix<int,VC,STAR>
pT(g), p0(g),
pB(g), p1(g),
p2(g);
// Temporary distributions
DistMatrix<F, STAR,STAR> A11_STAR_STAR(g);
DistMatrix<F, MC, STAR> A21_MC_STAR(g);
DistMatrix<F, STAR,VR > A12_STAR_VR(g);
DistMatrix<F, STAR,MR > A12_STAR_MR(g);
DistMatrix<int,STAR,STAR> p1_STAR_STAR(g);
// Pivot composition
std::vector<int> image, preimage;
// Start the algorithm
PartitionDownDiagonal
( A, ATL, ATR,
ABL, ABR, 0 );
PartitionDown
( p, pT,
pB, 0 );
while( ATL.Height() < A.Height() && ATL.Width() < A.Width() )
{
RepartitionDownDiagonal
( ATL, /**/ ATR, A00, /**/ A01, A02,
/*************/ /******************/
/**/ A10, /**/ A11, A12,
ABL, /**/ ABR, A20, /**/ A21, A22 );
RepartitionDown
( pT, p0,
/**/ /**/
p1,
pB, p2 );
View1x2( AB, ABL, ABR );
const int pivotOffset = A01.Height();
A12_STAR_VR.AlignWith( A22 );
A12_STAR_MR.AlignWith( A22 );
A21_MC_STAR.AlignWith( A22 );
A11_STAR_STAR.ResizeTo( A11.Height(), A11.Width() );
p1_STAR_STAR.ResizeTo( p1.Height(), 1 );
//--------------------------------------------------------------------//
A21_MC_STAR = A21;
A11_STAR_STAR = A11;
lu::Panel( A11_STAR_STAR, A21_MC_STAR, p1_STAR_STAR, pivotOffset );
ComposePivots( p1_STAR_STAR, pivotOffset, image, preimage );
ApplyRowPivots( AB, image, preimage );
// Perhaps we should give up perfectly distributing this operation since
// it's total contribution is only O(n^2)
A12_STAR_VR = A12;
LocalTrsm
( LEFT, LOWER, NORMAL, UNIT, F(1), A11_STAR_STAR, A12_STAR_VR );
A12_STAR_MR = A12_STAR_VR;
LocalGemm( NORMAL, NORMAL, F(-1), A21_MC_STAR, A12_STAR_MR, F(1), A22 );
A11 = A11_STAR_STAR;
A12 = A12_STAR_MR;
A21 = A21_MC_STAR;
p1 = p1_STAR_STAR;
//--------------------------------------------------------------------//
A12_STAR_VR.FreeAlignments();
A12_STAR_MR.FreeAlignments();
A21_MC_STAR.FreeAlignments();
SlidePartitionDownDiagonal
( ATL, /**/ ATR, A00, A01, /**/ A02,
/**/ A10, A11, /**/ A12,
/*************/ /******************/
ABL, /**/ ABR, A20, A21, /**/ A22 );
SlidePartitionDown
( pT, p0,
p1,
/**/ /**/
pB, p2 );
}
}
