//************************************************************************* 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 ); } }